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ABSTRACT 

This  Software  Reference  documents  and  summarizes  all  source  code  produced  for  a 
Ph.D.  dissertation  constructing  an  underwater  virtual  world  for  an  Autonomous  Underwater 
Vehicle  (AUV). 

A  critical  bottleneck  exists  in  Autonomous  Underwater  Vehicle  (AUV)  design  and 
development.  It  is  tremendously  difficult  to  observe,  communicate  with  and  test  underwater 
robots,  because  they  operate  in  a  remote  and  hazardous  environment  where 
physical  dynamics  and  sensing  modalities  are  counterintuitive. 

An  underwater  virtual  world  can  comprehensively  model  all  salient  functional 
characteristics  of  the  real  world  in  real  time.  This  virtual  world  is  designed  from  the 
perspective  of  the  robot,  enabling  realistic  AUV  evaluation  and  testing  in  the  laboratory. 
Three-dimensional  real-time  computer  graphics  are  our  window  into  that  virtual  world. 

Visualization  of  robot  interactions  within  a  virtual  world  permits  sophisticated  analyses 
of  robot  performance  that  are  otherwise  unavailable.  Sonar  visualization  permits  researchers 
to  accurately  "look  over  the  robot’s  shoulder"  or  even  "see  through  the  robot’s  eyes"  to 
intuitively  understand  sensor-environment  interactions.  Extending  the  theoretical  derivation 
of  a  set  of  six-degree-of-freedom  hydrodynamics  equations  has  provided  a  fully  general 
physics-based  model  capable  of  producing  highly  non-linear  yet  experimentally-verifiable 
response  in  real  time. 

Distribution  of  underwater  virtual  world  components  enables  scalability  and  real-time 
response.  The  IEEE  Distributed  Interactive  Simulation  (DIS)  protocol  is  used  for  compatible 
live  interaction  with  other  virtual  worlds.  Network  connections  allow  remote  access, 
demonstrated  via  Multicast  Backbone  (MBone)  audio  and  video  collaboration  with 
researchers  at  remote  locations.  Integrating  the  World-Wide  Web  allows  rapid  access  to 
resources  distributed  across  the  Internet. 

This  dissertation  presents  the  frontier  of  3D  real-time  graphics  to  support  underwater 
robotics,  scientific  ocean  exploration,  sonar  visualization  and  worldwide  collaboration. 
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1.  INTRODUCTION 

The  purpose  of  this  guide  is  to  document  the  source  code  developed  in  the 
creation  of  an  underwater  virtual  world  for  an  autonomous  underwater  vehicle  (AUV). 
It  is  companion  document  to  a  dissertation  (Brutzman  94).  There  are  a  number  of 
parts  to  this  work.  First  a  users  guide  is  presented  which  describes  how  to  obtain  and 
install  the  software  described  here.  Key  mission  output  files  then  show  results 
produced  by  the  autonomous  underwater  robot  interacting  with  the  virtual  world. 
Remaining  chapters  present  the  source  code  developed  for  robot  execution  software, 

3D  computer  graphics,  six  degree-of-freedom  hydrodynamics,  a  geometric  sonar 
model,  networking,  and  hypermedia  using  the  World-Wide  Web  and  the  Multicast 
Backbone  (MBone). 

Several  languages  are  included  in  this  source  code.  Robot  execution  programs 
and  supporting  configuration  files  are  written  in  the  Kemigan  and  Richie  variant  of  the 
C  language,  and  run  identically  under  Irix  5.2  or  OS-9  operating  systems.  Other 
source  programs  are  written  primarily  in  C++.  Source  code  file  name  conventions  use 
a  extension  for  C  language  files,  and  a  .C  extension  for  C++  language  files. 
Remaining  programs  and  files  are  written  in  languages  specific  to  the  application,  such 
as  gnuplor  (Williams  95),  Tool  control  language  (Tel)  (Osterhout  94)  (Welch  94), 
hypertext  markup  language  (.html)  (Berners-Lee  94a,  94b)  (Flughes  94)  or  operating 
system  shell  scripts. 

All  of  the  programs  described  here  are  available  electronically  without  charge  to 
Internet  users  via  anonymous  ftp.  The  accompanying  public  electronic  distribution  of 
this  reference  includes  both  source  code  and  compiled  executable  programs.  All 
programs  have  been  tested  under  SGI  operating  system  Irix  5.2,  and  robot  code  has 
also  been  tested  under  operating  system  OS-9  version  2.3.  Goals  of  the  underwater 
virtual  world  project  include  making  source  code,  applications  and  interfaces  easily 
available  in  order  to  encourage  scientific  collaboration  and  educational  use.  Future 
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work  includes  porting  virtual  world  viewers  to  a  variety  of  operating  systems  and 
computer  architectures. 

An  NPS  AUV  hypermedia  home  page  has  been  prepared  to  facilitate 
examination  and  download  of  the  various  underwater  virtual  world  components.  This 
home  page  is  available  for  interaction  using  any  number  of  Internet-based  browsers, 
most  notably  Mosaic  (NCSA  94a).  Resources  on  the  home  page  include  the  electronic 
distribution,  installation  and  execution  instructions,  pointers  to  related  work,  images, 
plots,  papers  and  a  variety  of  other  media.  World-Wide  Web  (WWW)  Uniform 
Resource  Locator  (URL)  for  the  NPS  AUV  home  page  is: 
ftp:lltaurus.cs.nps.navy.millpublauvlauv.html 

To  learn  more  about  the  World-Wide  Web,  consult  "Entering  the  World-Wide 
Web  (WWW):  A  Guide  to  Cyberspace"  (Hughes  94).  This  paper  describes  everything 
needed  to  start  using  Mosaic,  accessing  all  types  of  information  sources,  and  using 
hypermedia  and  the  Internet.  Mosaic  is  a  hypermedia  browser  that  is  freely  available 
for  Unix  machines  running  X  windows,  Macintoshes  and  personal  computers  (PCs) 
running  Windows.  There  are  also  free  line  mode  browsers  (such  as  lynx  and  www) 
that  let  you  browse  hypertext  documents  in  a  simple  text  mode  using  any  computer 
which  has  an  internet  connection  (Montulli  94)  (Berners-Lee  94b). 

The  source  code  in  this  software  guide  is  fully  documented  and  reasonably 
self-explanatory.  Graphics  and  hydrodynamics  programs  are  written  in  the  ANSI 
standard  C-1-+  language,  robot  execution  code  and  networking  code  is  written  in  the 
Kernighan  and  Richie  variant  of  the  C  language,  plotting  programs  use 
gnuplot  version  3.5  scripting  language,  and  hypenext  pages  are  written  in  standard 
Hypertext  Markup  Language  (.html)  (NCSA  94b).  Despite  extensive  documentation, 
however,  one  programmer’s  clearly-written  code  may  well  be  undecipherable  to 
another  programmer.  Users  are  strongly  encouraged  to  review  the  companion 
dissertation  which  derives  the  formulae  and  explains  the  concepts  behind  the  software 
(Brutzman  94),  retrieve  the  latest  version  of  the  underwater  virtual  world  distribution, 
or  contact  the  author  for  questions  that  remain  unanswered.  No  guarantees  of  any 
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kind  are  implied.  Many  problems  are  open  and  progress  occurs  on  a  frequent  basis 
when  conducting  research  on  robots  and  virtual  worlds. 

Hopefully  this  work  will  stimulate  and  encourage  other  research  efforts  to  build 
large-scale  virtual  worlds  in  compatibly-extendable  ways.  Interested  readers  are 
encouraged  to  examine  the  AUV  home  page  for  project  updates,  read  the 
documentation,  and  to  contact  the  author  regarding  new  uses  of  the  software  and 
potential  research  collaboration. 


3 


II.  INSTALLATION,  EXECUTION,  CREATION  AND  REMOVAL 


A.  Introduction 

The  following  Installation  and  Execution  Guide  is  intended  to  enable  any  new 
user  to  download,  install,  execute  and  nndersiand  tbe  NFS  AUV  Underwater  Virtual 
World.  Users  may  want  to  solely  download  the  viewer  software  to  observe  an 
Internet-wide  mission  exercise,  or  run  the  entire  set  of  programs  making  up  the  virtual 
world.  Users  of  the  robot  execution  program  can  enter  their  electronic  mail  address 
and  receive  a  mission  report  following  each  mission.  A  sample  mission  report  shows 
the  mission  script  used  for  the  canoscical  SIGGRAPH  94  mission,  a  project  abstract, 
pointers  to  related  work  and  the  execution  orders  generated  by  the  mission  script. 
Mission  reports  are  also  available  by  "fingering"  the  home  auv  account,  e.g. 

finger  auv(aducJe.cs.nps.navy.fnil 

Finally,  shell  script  files  for  archive  creation  and  removal  are  included  to 
document  automated  archive  maintenance. 
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B.  auv-uvw.  INSTALL  Installation  and  Execution  Guide 


Distribution  information 
System  recfuirements 

Retrieving  the  distribution  auv-uvw. tar . Z 
Installing  the  distribution  auv-uvw .tar . Z 
MBone  connection  (optional  but  recommended) 

Information  files 

Viewing  remotely- executing  robot  missions  via  the  MBone 
Running  all  of  the  virtual  world  components  locally 
Plotting  mission  telemetry  using  gnuplot 
Recompiling  virtual  world  component  executables 
Killing  multicast  processes 
Removing  the  entire  distribution 
Future  work 
Contact  information 

This  is  a  big  project!  I've  tried  to  make  this  guide  as  concise  and  readable 
as  possible.  However  some  work  is  necessary  to  read  and  absorb  this  material. 


Suggestions  on  how  to  further  streamline  the  guide  are  very  welcome.  Thanks! 


Distribution  information 

For  the  latest  greatest  distribution,  plus  pointers  to  all  of  the  supporting 
public  domain  programs,  use  Mosaic  or  a  World-Wide  Web  line  browser  to 
connect  to  the  NPS  AUV  home  page: 

ftp : //taurus . cs .nps.navy.mil/pub/auv/auv.html 

To  retrieve  the  distribution  directly: 

ftp : / /taurus . cs . nps . navy . mil /pub/auv/ auv-uvw . tar . Z 

To  receive  a  copy  of  a  recent  mission  report: 

finger  auv®dude . cs .nps .navy .mil 


System  requirements 

The  auv-uvw  'viewer'  requires  SGI  Irix  5.2  or  better  with  Openinventor  2.0 
Execution  Only  Environment  (inventor_eoe)  installed.  This  is  provided 
free  with  all  versions  of  Irix,  it  merely  needs  to  be  installed. 

You  can  tell  if  it  is  installed  by  typing  'versions'  at  the  SOI  protrq>t. 

Modification  and  recompilation  of  the  'viewer'  requires  installation  of 
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inventor_dev  Inventor  3D  Toolkit,  2.0  development  option. 

The  virtual  world  'viewer'  is  written  in  C++  using  Openinventor 
graphics,  and  it  also  uses  the  NPSNET  DIS  2.0.3  multicast  libraries 
(which  are  also  included  as  source/binaries  in  this  distribution) . 

The  'dynamics'  virtual  world  software  has  been  tested  under  Irix  5.2, 
is  written  in  C++,  and  also  uses  the  NPSNET  DIS  2.0.3  multicast 
libraries .  You  should  run  it  on  a  sound- equipped  workstation  for 
best  results . 

The  robot  'execution'  code  has  been  tested  under  Irix  5.2  and  OS-9  v2.3 
It  is  written  in  ANSI  C  with  special  #defines  for  OS- 9  K&R  C 
compatibility,  allowing  it  to  be  run  without  modifications  either  under 
Irix  or  on  the  NPS  Autonomous  Underwater  Vehicle  68030  microprocessor. 
It  ought  to  be  portable  to  most  any  plain  vanilla  C  compiler. 


//  — . - . - . - . // 

Retrieving  the  distribution  auv-uvw . tar . Z 

The  easiest  way  is  to  retrieve  &  save  the  tar  file  is  to  point  Mosaic  at 

ftp : //taurus . cs .nps .navy.mil/pub/mosaic/auv.html 

If  instead  you  use  anonymous  ftp,  here  is  a  sample  session: 

unix>>  ftp  taurus.cs.nps.navy.mil 
Connected  to  taurus.cs.nps.navy.mil. 

220  taurus  FTP  server  (Version  6.10  Wed  Mar  18  11:57:03  PST  1992)  ready. 

Name  ( taurus . cs . nps . navy .mil: yourname ) :  anonymous 
331  Guest  login  ok,  send  e-mail  address  as  password. 

Password :  your . email . address . here 

ftp>  cd  pub/auv 

250  CWD  command  successful. 

ftp>  binary 

200  Type  set  to  I. 

ftp>  get  auv-uvw . tar . 2 

local:  auv-uvw . tar . Z  remote:  auv-uvw . tar . Z 

150  Opening  BINARY  mode  data  connection  for  auv-uvw . tar . Z (4747669  bytes) 

226  Transfer  complete. 

4747669  bytes  received  in  34.32  seconds  (135.08  Kbytes/s) 
ftp>  quit 

221  Goodbye. 

The  uncompressed  distribution  is  about  10  MB,  not  counting  the  extra 
recommended  applications.  The  compressed  distribution  is  about  5  MB. 
Precise  distribution  sizes  can  vary  when  changes  are  incoirporated. 


// . // 

Installing  the  distribution  auv-uvw . tar . Z 


Uncompress  auv-uvw .tar . Z  in  your  root  directory  by  typing 
Unix : yourhome >  uncompress  auv-uvw. tar .Z 

Unix: your home >  tar  -xvf  auv-uvw. tar 


Backup  &  update  your  MBone  session  directory  configuration  file  .sd.tcl 
Unix : yourhome >  cp  .sd.tcl  .sd.tcl. old 
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Unix : your home >  cp  ,sd.tcl,auv-uvw  .sd.tcl 


Edit  the  MBone  tool  aliases  to  include  directory  paths  as  appropriate . 

They  are  located  at  the  very  end  of  the  new  file  .sd.tcl 

Unix : yourhome >  zip  .sd.tcl 

After  editing,  exit  and  restart  session  directory  (sd) : 

Unix : yourhome  >  sd 

You  can  now  run  the  viewer  from  an  sd  advert isment  for  the  NPS  AUV  Underwater 
Virtual  World  (if  there  is  one  running  -  this  is  optional) . 


You  will  notice  some  auv-uvw  information  files  in  your  root  directory 
and  several  new  directories: 

yourhome /auv-uvw/  Openinventor  viewer  program  &  data  files 

yourhome /executi on /^UV  (robot)  execution  level  code  and 
mission  scripts 

your  home /dynamics /Virtual  world  for  robot:  dynamics,  sonar 
and  other  components 

yourhome/dynamics/speeclText- to- speech  audio  files  from  mission 
scripts.  These  originate  from  queries 
to  the  Say. .  server 


The  auv-uvw  distribution  should  now  be  fully  installed. 


// . - . // 

MBone  connection  (optional  but  recommended) 

Multicast  Backbone  (MBone)  connectivity  is  needed  if  you  want 
to  participate  in  remote  exercises.  A  good  way  to  test  your  network's 
connectivity  is  to  type  'sd'  (session  directory).  If  you  get  a  menu 
of  MBone  programs  starting  to  slowly  build  up,  you  are  in  business. 

To  receive  Distributed  Interactive  Simulation  (DIS)  robot  execution 
Protocol  Data  Units  (PDUs)  originating  on  your  own  network,  you 
do  not  have  to  be  connected  to  the  Internet-wide  MBone. 

SGI's  Irix  5.2  operating  system  supports  multicast  in  the  kernel,  so 
you  can  still  run  the  robot/virtual  world  programs  on  one  machine  & 
the  auv-uvw  viewer  on  another.  Only  one  process  per  machine  can  grab 
the  multicast  port,  thus  at  least  2  machines  are  needed  to  run 
everything . 

To  connect  your  system  to  the  MBone  takes  some  work  but  is  worth  it. 

You  will  need  the  involvement  &  support  of  your  network  administrator. 

The  following  article  tells  why  MBone  is  so  great  and  how  to  connect: 

ftp : //taurus . cs .nps .navy .mil/pub/mbmg/mbone .ps  (PostScript)  or 
ftp://tauru8.cs.nps.navy.mil/pub/mbmg/mbone.html  (hypertext)  or 
ftp://tauru8.c8.nps.navy.mil/pub/mbmg/mbone.txt  (ASCII  text) 

Other  installation  pointers  are  on  the  NPS  MBone  home  page  at 
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ftp : //taurus . cs .nps .navy.mil/pub/mosaic/mbone .html 

Over  a  thousand  subnets  worldwide  are  already  connected  to  the  MBone 
There  are  lots  of  people  willing  to  help.  In  practice  we've  seen  it 
take  people  between  a  few  days  and  a  few  weeks  to  start  using  MBone, 
but  once  connected  further  work  is  rarely  required.  Good  luck! 


//  — . - . . . . // 

Information  files : 

auv-uvw. virtual -world. README  (a  mission  report) 

auv-uvw. virtual- world. INSTALL  (this  file) 

others  are  listed  in  the  mission  report  and  the  home  page  site 


// . . --- . - . . . . // 

Viewing  remotely-executing  robot  missions  via  the  MBone 

You  must  have  MBone  connectivity  installed  on  your  network  for  remote 
experiment  observation.  If  the  NFS  AUV  Underwater  Virtual  World 
is  advertised  in  sd,  and  you  have  installed  the  distribution, 
just  select  the  session  and  multicast  ports  are  passed  automatically 
to  the  viewer. 

Alternatively,  you  can  start  the  viewer  manually  from  the  command  line: 

Unix : yourhome >  auv-uvw .viewer 

or  Unix:  yourhome /auv-uvwsy'iewer 

The  following  command  line  switches  are  available  with  viewer: 

-address  224 , 2 . ###  .  ##nulticast  address  as  advertised  in  sd  session 
-port  #####  multicast  wb  port  as  advertised  in  sd  session 

The  virtual  world  viewer  should  now  run. 


// . // 

Riinning  all  of  the  virtual  world  components  locally 

Running  a  robot  mission  takes  three  processes  and  at  least  two  machines . 
viewer  and  dynamics  must  be  on  different  hosts,  execution  can  run  anywhere. 

Notes  regarding  sd  session  advertisements  are  optional. 

*  First  is  the  'viewer'  to  get  a  window  into  the  virtual  world.  Run  this 
on  your  most  capable  Iris  workstation  -  Reality  Engines  are  nice ! 

You  can  use  or  disable  texturing  based  on  the  capabilities  of  the  machine. 

unixl>  cd  auv-uvw 
unixl>  viewer 

The  following  optional  command  line  switches  are  available  with  viewer: 

-address  224 . 2  .  ### .  ##nulticast  address  as  advertised  in  sd  session 
-port  #####  multicast  wb  port  as  advertised  in  sd  session 

-texture -on 
-texture -off 

-print dialog  pop  up  print  dialog  box  for  screen  snapshots 
-noprint dialog 

The  virtual  world  viewer  now  runs . 
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*  The  'dynamics'  program  is  the  virtual  world  connection  for  robot  execution: 
Run  it  on  a  sound- equipped  workstation  for  best  results,  otherwise  ignore  any 
sound- card-missing  error  messages.  The  'dynamics'  program  is  purely 
text-based  so  graphics  capabilities  are  irrelevant. 

unix2>  cd  dynamics 
unix2>  dynamics 

Dynamics  classes  test  selections 

L  loop__test_with_execution_level  (); 

M  Multicast  parameter  input 

ttl=15,  group  address=224.2.121.93,  port==3111 
O  Ocean  current  vector  reset  <0,  0,  0> 

H  Hmatrix/quatemion  exerciser 

R  Rotation  of  quaternion  &  Hmatrix  using  p  q  r 
D  Defaults 
I  Invert  matrix  test 

£  dEad_reckon_test_with_execution_level 
P  PDU_skip_interval  change  (from  1) 

T  Toggle  TRACE  =  0 
C  DIS_net_close  (); 

Q  Quit 

Enter  choice : 

Select  L  for  loop  test  with  execution  level 

The  following  command  line  switches  are  available  with  dynamics  program: 

-ttl  #  time  to  live  (be  careful!)  recommend  16  or  less 

-address  224 . 2 .  ### .  ##nulticast  address  as  advertised  in  sd  session 
-port  #####  multicast  wb  port  as  advertised  in  sd  session 

-loop  start  looping  automatically 

♦  Finally,  'execution'  is  the  robot  execution  level  software. 

'execution'  runs  through  the  file  'mission . script '  and  gets 
real  world  responses  from  the  virtual  world  program  'dynamics' . 

'execution'  can  run  under  Unix  or  real-time  operating  system  OS-9. 

unix3>  cd  execution 

unix3>  cp  mission . script . the_one_you_^want  mission . script 

unix3>  execution  remotehost  unixl .host .name 

Enter  your  e-mail  in  the  execution  process  window  when  asked. 

See  the  file  mission. script .HELP  for  info  on  how  to  write  mission  scripts. 


Plotting  mission  telemetry  using  gnuplot 
uni x3 : your home >  cd  execution 

unix3  :yourhome/execution/3gnuplot  auv_plot  .gnu 
For  fewer  points  plotted  (1  per  second)  out  of  the  same  dataset: 
unix3  :  yourhome/execution/:gnuplot  auv_plot_l_second . gnu 


or  use  a  postscript  viewer  (3q)8view  or  ghostview)  to  look  at  plots  directly: 

unix3>  xpsview  -wp  -skipc  -or  landscape  -/execution/AUV^telemetry .ps  & 
or  unix3>  ghostview  -landscape  - /exe cut ion/AUV_te lemetry.ps  & 


// 


// 
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Recompiling  virtual  world  component  executables 
auv-uvw  directory 

(Requires  inventor_dev  Inventor  2.0  development  option  installed. 
See  system  requirements  section  for  details) . 

unix3 lyourhome /auv-uvw/ >  make  viewer 


dynamics  directory 

unix3 lyourhome /dynamics />  make  dynamics 


execution  directory 

unix3 : your home/ exe cut ion/ >  copy  Makefile. SGI  Makefile 
unix3 lyourhome /execution/ >  make  execution 

or  OS- 9;  yourhome/execution/>  copy  Makefile. OS 9  Makefile 
OS-9:  yourhome/execution/>  make  execution 


Details  on  recompilation  and  modification  are  included  in  all  source  files. 

// - - -  - . . . . . . // 

Killing  multicast  processes 

Sometimes  multicast  processes  (such  as  'viewer'  and  'dynamics')  do  not  die 
cleanly  because  they  have  forked  a  second  multicast  process, 
viewer  is  especially  prone  to  this  behavior  when  invoked  from  session 
directory  (sd) ,  since  sd  must  put  the  viewer  process  into  the  background. 

You  may  need  to  take  special  action  to  ensure  the  processes  are  killed. 

The  following  instructions  will  work  on  SGI  machines : 

To  put  a  process  to  sleep  that  refuses  to  let  go  of  the  command  prompt: 

type  a  '"Z  (control-Z)  in  the  runaway  process  window 
To  see  what  processes  are  running: 
unix>  ps 

To  see  ALL  processes  that  are  running  (including  previous  zombie  processes) : 

unix>  ps  -ea 
To  kill  a  process: 

unix>  kill  -9  ####  where  ####  is  the  Process  ID  (PID) 


An  example  log: 

[runaway  process,  you  hit  the  control-Z  key  to  suspend  it] 
^Z 

Suspended 
unix>>  ps 


PID 

TTY 

TIME 

COMD 

16558 

ttyq3 

9:37 

viewer 

11289 

ttyq3 

0:01 

tcsh 

16581 

ttyq3 

0:00 

viewer 

16902 

ttyq3 

0:00 

ps 
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\inix>>  kill  -9  16558  16581 


uiiix>>  ps 

PID  TTY 
11289  ttyq3 
16904  ttyq3 


TIME  COMD 
0:01  tcsh 
0:00  ps 


// . // 

Removing  the  entire  distribution: 

Be  sure  to  move  any  files  that  you  want  to  save  to  different  directories. 

You  do  not  have  to  remove  the  old  distribution  prior  to  updating 
to  a  new  version.  However  you  might  want  to,  so  that  superceded  files 
in  old  distributions  are  not  left  over. 

To  remove  everything,  run  the  shell  script  auv-uvw .virtual -world .REMOVE : 

unix>  source  auv-uvw. virtual -world. REMOVE 


// . - - - -  - .  -// 

Future  work  (lots!) 

Porting  the  Openinventor  viewer  code  to  other  architectures  and 
porting  other  virtual  world  components  to  other  architectures 
depends  on  availability  of  Openinventor  and  multicast  TCP/IP  support. 
Porting  is  planned  for  1995. 

Other  people  are  always  welcome  to  use  and  extend  this  code . 

Please  keep  me  informed  of  your  efforts  if  you  find  it  of  value. 

Other  research  collaborations  to  extend  the  underwater  virtual  world 
to  build  up  a  truly  large-scale  Internet-wide  fully  distributed 
virtual  world  are  of  particular  interest . 

Integrate  Dr.  Larry  Ziomek's  Recursive  Ray  Acoustics  (RRA)  sonar  model. 

Incorporate  pre-processed  terrain  datasets  for  Monterey  Bay. 

Virtual  Reality  Modeling  Language  (VRML)  -  see  the  working  group 
http : / /www . wired . com/vrml/ 

Dissertation  "A  Virtual  World  for  an  Autonomous  Underwater  Vehicle" 
is  being  written  and  will  be  made  publicly  available. 


// . // 

Contact  information: 

Don  Brut  zman  brut 2man®nps . navy . mil 

Code  OR/Br  Naval  Postgraduate  School  [Glasgow  204]  work  (408)  656-2149 
Monterey  California  93943-5000  USA  fax  (408)  656-2595 


// 


// 
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C.  mission.outpuL email  Mission  Output  Electronic  Mail  Report 


The  following  report  is  sent  by  electronic  mail  to  users  who  execute  a  robot 
mission.  It  demonstrates  that  robots,  once  networked,  can  report  back  results  only 
when  they  are  of  interest  or  human  intervention  is  specifically  desired.  The  goal  is 
for  the  robot  to  meet  user  directives  conveniently,  rather  than  requiring  constant 
monitoring  or  supervision. 

The  mission  report  combines  an  NPS  AUV  reference  information  file,  the 
mission  script  that  drove  the  robot,  and  the  resulting  time  line  of  propulsor  and  plane 
surface  orders.  It  is  also  available  by  querying  the  robot  account  with  the  Unix 
"finger"  command:  finger  auv@cs.dude.nps.navy.mil 


NPS  AUV  Mission  Report 

Here  is  a  mission  report  generated  by  the  Naval  Postgraduate  School  (NPS) 
Autonomous  Underwater  Vehicle  (AUV)  running  in  an  Underwater  Virtual  World. 

This  message  was  created  by  the  operating  robot  connected  to  the  network. 
Actual  robots  with  network  connections  can  send  reports  such  as  this 
to  research  teams  and  interested  individuals  when  scientific  discoveries 
are  made  or  unusual  circumstances  occur. 


virtually  yours,  the  NPS  AUV. 


8~/  nps  auv  ) 
8“\ _ ) 


The  following  Universal  Resource  Locators  (URL's)  are  pointers  that  lead  to 
more  information  about  the  NPS  AUV  Underwater  Virtual  World  &  related  work. 

*  The  AUV  underwater  virtual  world  source  code,  information  files  and 

*  executable  programs  (for  SGI  Irix  5.2  machines)  can  be  used  to  monitor 

*  NPS  AUV  DIS  PDU's  from  anywhere  on  the  Internet  MB one .  It  is  freely 

*  available  via  anonymous  ftp  as  a  compressed  tar  file  at 
ftp://taurus .cs .nps .navy .mil /pub/auv/auv_uvw . tar . Z 

*  NPS  AUV  home  page  free  software  distribution,  research  summary  and 

*  anonymous  ftp  directory: 

ftp : //taurus . cs .nps .navy.mil/pub/mosaic/auv.html 
ftp: //taurus . cs , nps .navy.mil/pub/auv/AUVPAPERS 
ftp: //taurus .cs.nps.navy.mil/pub/auv/ 

*  Naval  Postgraduate  School  World-Wide -Web  (WWW)  home  page  includes  many 

*  additional  pointers  to  the  NPSNET  Virtual  Battlefield,  DIS,  MBone,  and 

*  I3IiA  regional  research  around  Monterey  Bay: 

ftp : //taurus . cs .nps .navy .mil /pub/mos a ic/nps jmosaic.html 

*  To  learn  about  Internet  audio  &  video  on  the  Multicast  Backbone  (MBONE) : 
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ftp :  / /taurus .  C8 .  nps  .  navy .  mil/pub/mosaic/mbone .  html 

♦  How  the  robot  voice  was  synthesized  live  from  text  over  the  Internet  using 

♦  Axel  Belinfante's  **Say..."  speech  server  at  University  of  Twente, 

♦  Netherlands  which  uses  Nick  Ing- Simmons'  phoneme  synthesizer  'rsynth' : 
http: //utisl79 .cs.utwente .nl : 8001/say/? 

♦  Postscript  plots  (20  pages)  of  vehicle  telemetry  during  SIGGRAPH  mission: 

ftp : //taurus . cs .nps .navy.mil/pub/auv/SIQGRAPH94/AUV_telemetry.ps . Z 

♦  The  people  from  NPS  involved  in  SIGGRAPH  94  _The  Edge_  NPS  AUV  exhibit : 
ftp :// taurus . cs .nps.navy.mil/pub/pratts/home.html 

♦  Xjeaming  more  about  the  World-Wide -Web,  URLs  and  Mosaic: 

♦  "Entering  the  World-Wide  Web:  A  Guide  to  Cyberspace"  by  Kevin  Hughes 

http : //www . eit . com/web/www . guide/  hypertext 

ftp : //ftp . eit . com/pub/web .guide/guide . 61/guide , 61 .ps . 2  Postscript 
ftp : //ftp .eit . com/pub/web. guide/guide . 61/guide . 61.txt  text 

♦  A  recent  copy  of  this  mission  report  can  be  found  by  an  auv  account  finger: 
finger  auv®dude . cs . nps . navy . mil 


If  you  can't  find  taurus.c8.nps.navy.mil,  it  has  IP  number  131.120.1.13 
Additional  references  are  available  on  request . 


This  project  is  part  of  a  PhD  dissertation.  Here  is  the  abstract: 

A  Virtual  World  for  an  Autonomous  Underwater  Vehicle 
Don  Brutzman 

Code  OR/Br,  Naval  Postgraduate  School 
Monterey  California  93943-5000  USA 
(408)  656-2149  office,  (408)  656-2595  fax 
brut  zman®nps . navy . mi 1 

A  critical  bottleneck  exists  in  Autonomous  Underwater  Vehicle  (AUV) 
design  and  development.  It  is  tremendously  difficult  to  observe,  communicate 
with  and  test  \inderwater  robots,  because  they  operate  in  a  remote  and 
hazardous  environment  where  physical  dynamics  and  sensing  modalities  are 
counterintuitive . 

An  underwater  virtual  world  can  comprehensively  model  all  salient 
functional  characteristics  of  the  real  world  in  real  time.  This  virtual 
world  is  designed  from  the  perspective  of  the  robot,  enabling  realistic  AUV 
evaluation  and  testing  in  the  laboratory.  3D  real-time  graphics  are  our 
window  into  that  virtual  world.  Visualization  of  robot  interactions  within 
a  virtual  world  permits  sophisticated  analyses  of  robot  performance  that 
are  otherwise  unavailable.  Sonar  visualization  permits  researchers  to 
accurately  "look  over  the  robot's  shoulder"  or  even  "see  through  the 
robot's  eyes"  to  intuitively  understand  sensor- environment  interactions. 

Distribution  of  underwater  virtual  world  components  enables  scalability 
and  real-time  response.  The  IEEE  Distributed  Interactive  Simulation  (DIS) 
protocol  is  used  for  compatible  live  interaction  with  other  virtual  worlds. 
Network  access  allows  individuals  remote  access .  This  is  demonstrated  via 
MBONE  collaboration  with  others  outside  The  Edge,  and  Mosaic  access  to 
pertinent  archived  images,  papers,  cUtasets,  software,  sound  clips,  text 
and  any  other  con^uter- storable  media. 

This  project  presents  the  frontier  of  3D  real-time  graphics  for 
underwater  robotics,  ocean  e3q>loration,  sonar  visualization  and  worldwide 
scientific  collaboration. 


Questions,  comments  and  collaborations  are  welcome.  Please  contact: 
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Don  Brutzman  brut zman®np s . navy .mil 

Code  OR/Br  Naval  Postgraduate  School  [Glasgow  204]  work  (408)  656-2149 
Monterey  California  93943-5000  USA  fax  {A08)  656-2595 

SIGGRAPH  94  Collaborators:  Michael  J.  Zyda,  Paul  T.  Barham,  John  S.  Falby, 

Anthony  J.  Healey,  Shirley  Isakari,  Rodney  Luck, 
Michael  R.  Macedonia,  Robert  B.  Mcghee, 

David  R.  Pratt,  Lawrence  J.  Ziomek 

Naval  Postgraduate  School,  Monterey  California 


Your  AUV  robot  mission  completed  successfully.  Here  is  the  mission  you  ran: 

#  your  mission  is 

#  mission . script .rotate_test 

time  0 

timestep  0.1 

#  initialize  vehicle 
depth  45 

position  0  0  45 

orientation  000 

thrusters-on 

rotate  16 

pause 

wait  60 

rotate  0 

thrusters -off 

wait  120 

step 

keyboard 

keyboard-off 

#  mission  complete 
kill 

NPS  AUV  file  mission . output . orders :  commanded  propulsion  orders  versus  time 

# 

#  timestep:  0.10  seconds 

# 


#  time  heading  North 

Hast 

Depth 

rpm 

rpm 

stern 

stern 

vertical 

lateral 

#  X 

y 

z 

port 

stbd 

plane 

rudder 

thrusters 

thrusters 

# 

bow/s tern 

bow/stern 

0.0  0.0  0.0 

0.0 

0.0 

0.0 

0.0 

0.0 

0.0 

0.0 

0.0 

0.0  0.0 

#  timestep:  0.10  seconds 

0.0  0.0  0.0 

0.0 

45.0 

0.0 

0.0 

0.0 

0.0 

0.0 

0.0 

0.0  0.0 

60.0  0.0  0.0 

0.0 

45.0 

0 . 0 

0.0 

0.0 

0.0 

0.0 

0 . 0 

0.0  0.0 

180.1  0.0  0.0 

0.0 

45.0 

0.0 

0.0 

0.0 

0.0 

0.0 

0.0 

0.0  0.0 
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D.  make _auv_uvw Jar  Archiye  Creation  Script 

#  make_auv_uvw_tar :  NPS  AUV  Underwater  Virtual  World  archive  creation  script 

#  Don  Brutzman  20  OCT  94 

echo  create  auv  underwater  virtual  world  distribution  tar  file  auv-uvw. tar . Z 

#  Notes: 

#  do  not  make  this  tar  on  gravyl,  it  doesn't  have  enough  memory  for  speech 

#  ensure  mission. script .siggraph  was  the  latest  (best)  mission  run 

#  this  script  is  not  included  in  the  distribution. 

#  NPS  Autonomous  Underwater  Vehicle  (AUV)  World-Wide  Web  Home  Page 

#  ftp: //taurus .cs.nps .navy.mil/pub/mosaic/auv.html 

cd  /n/dude /work/brut zman 

echo  'removing  old  files. . . ' 
rm  -f  auv-uvw.tar 

rm  -f  auv-uvw. tar .Z 

#  Standard  e-mail  report  becomes  the  README  file 

^  .f  auv-uvw.  virtual -world.  README 

cp  execution/mission . output . email  auv-uvw. virtual -world. README 

chmod  -w  auv-uvw. virtual -world .README 

tar  -cvf  auv-uvw.tar  auv-uvw. virtual -world. README 

rep  auv-uvw . virtual -world. README  taurus : -ftp/pub /auv 

chmod  +w  auv-uvw.  virtual -world.  README 

rm  -f  auv-uvw. virtual -world. README 


rm  -f 

cp  exe cut ion /auv-uvw . INSTALL 

chmod  -w 


auv-uvw .virtual -world . INSTALL 
auv-uvw .virtual -world , INSTALL 
auv-uvw .virtual -world . INSTALL 


auv-uvw .virtual -world. INSTALL  taurus : ~f tp/pub/auv 

auv-uvw . tar  auv-uvw .virtual -world . INSTALL 

auv-uvw . virtual -world . INSTALL 


rep  auv-uvw. vir 

tar  -rvf  auv-uvw.tar 
chmod  +w 
rm  -f 


rm  -f 

cp  exe cut ion /auv-uvw .REMOVE 

chmod  +x 

tar  -rvf  auv-uvw.tar 


auv-uvw .virtual -world. INSTALL 

auv-uvw .virtual -world .REMOVE 
auv - uvw .virtual- world . REMOVE 
auv-uvw . virtual -world . REMOVE 
auv-uvw . virtual -world . REMOVE 
auv-uvw .virtual -world . REMOVE 


execution/mission. script .HELPtaurus : -ftp/pub/auv 


-rvf  auv-uvw.tar  auv- uvw. viewer 

-rvf  auv-uvw.tar  auv -uvw/ viewer 
-rvf  auv-uvw.tar  auv-uvw/viewer . C 
-rvf  auv-uvw.tar  auv-uvw/Makef ile 
-rvf  auv-uvw.tar  auv -uvw/ over hang. rgb 
-rvf  auv-uvw.tar  auv -uvw/ auv. iv 
-rvf  auv-uvw.tar  auv-uvw/ Ja son. iv 
-rvf  auv-uvw.tar  auv-uvw/Platform. iv 
-rvf  auv-uvw.tar  auv-uvw/Testtank. iv 
-rvf  auv-uvw.tar  auv-uvw/testtank. C 

-rvf  auv-uvw.tar  auv.html 
-rvf  auv-uvw . tar  mbone , html 
-rvf  auv-uvw . tar  nps_jmosaic .html 

-rvf  auv-uvw.tar  .sd.t cl .auv-uvw 
-rvf  auv-uvw.tar  .mail cap .auv-uvw 


15 


tar  -rvf  auv-uvw.tar  . cshrc- excerpt . auv-uvw 

tar  -rvf  auv-uvw.tar  execution/execution 

tar  -rvf  auv-uvw.tar  execution/execution . c 

tar  -rvf  auv-uvw.tar  execution/parse_functions . c 

tar  -rvf  auv-uvw.tar  execution/globals . c 

tar  -rvf  auv-uvw.tar  execution/globals .h 

tar  -rvf  auv-uvw.tar  exe cut ion/de f ines .h 

tar  -rvf  auv-uvw.tar  execution/ermo .h 

tar  -rvf  auv-uvw.tar  execut ion /modes .h 

tar  -rvf  auv-uvw.tar  execution/setsys .h 

tar  -rvf  auv-uvw.tar  execution/sgstat .h 

tar  -rvf  auv-uvw.tar  execution/Makef ile* 

chmod  -w  execution/mission . script .HELP 

tar  -rvf  auv-uvw.tar  execution/mission.* 

chmod  644  execution/mission . script .HELP 

tar  -rvf  auv-uvw.tar  execution/auvjlot .gnu 

tar  -rvf  auv-uvw.tar  execution/auv_plot_l_second .gnu 

tar  -rvf  auv-uvw.tar  execution/print_plots 

tar  -rvf  auv-uvw.tar  execution/disbridge . c 

tar  -rvf  auv-uvw.tar  execution/os9sender . c 

tar  -rvf  auv-uvw.tar  execution/os9server . c 

tar  -rvf  auv-uvw.tar  dynamics /dynamics 
tar  -rvf  auv-uvw.tar  dynamics /Makefile 
tar  -rvf  auv-uvw.tar  dynamics/* .H 
tar  -rvf  auv-uvw.tar  dynamics/ *.C 

echo  'remove  speech/numbers . au  originating  from  high  replication  counts...' 

rm  -f  dynamic8/speech/2? . au 

rm  -f  dynamics/speech/3? .au 

rm  -f  dynamics/speech/4? . au 

rm  -f  dynamics/speech/5? .au 

rm  -f  dynamics/speech/6? .au 

rm  -f  dynamics/speech/7? . au 

rm  -f  dynamics/speech/8? ,au 

rm  -f  dynamics/speech/9? .au 

rm  -f  dynamics/speech/1?? . au 

rm  -f  dynamics/speech/2?? . au 

rm  -f  dynamics/speech/3?? . au 

rm  -f  dynamics/speech/4?? . au 

echo  'the  speech  directory  can  get  very  large  and  occasionally  needs  pruning...' 
tar  -rvf  auv-uvw.tar  dynamics/speech/* . au 

tar  -rvf  auv-uvw.tar  DIS.mcast/* 

#  The  following  files  are  provided  separately  to  reduce  tar  file  size. 

#  See  the  auv  home  page  for  remote  access  instructions 

#  ftp : //taurus . cs .nps .navy.mil/pub/mosaic/auv.html 

#  tar  -rvf  auv-uvw.tar  execution/AtA^_telemetry .ps 

#  tar  -rvf  auv-uvw.tar  aaai92ws .ps . Z 

#  tar  -rvf  auv-uvw.tar  dynamics/www 

#  tar  -rvf  auv-uvw.tar  execution/gnuplot 

Is  -1  auv-uvw.tar 

echo  'compressing  tar  file...' 

compress  auv-uvw.tar 

Is  -1  auv-uvw.tar . 2 

echo  'rep  auv-uvw . tar . 2  taurus  : -ftp /pxib/ auv' 
rep  auv-uvw. tar . Z  taurus : -ftp/pub/auv 

echo  "archive  information  on  taurus  anonymous  ftp  server;" 

rsh  taurus  'Is  -1  -ftp/pvib/auv/auv-uvw. tar . Z' 
e  cho  '  make_auv_uvw_t  ar  comp  1  e  t e  .  ' 
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E.  auv-uvw. virtual-wor1d.REMOVE  Archive  Removal  Script 


#  auv-uvw. virtual -world. REMOVE  archive  removal  shell  script 

#  To  ri«  this  script,  please  type  the  following  at  the  xmix  prompt 

#  chznod  +x  auv-uvw. virtual -world. REMOVE 

#  source  auv-uvw .virtual -world. REMOVE 

#  Don  Brutzman  19  OCT  94 

#  KPS  Autonomous  Underwater  Vehicle  (AUV)  World-Wide  Web  Home  Page 

#  ftp : / / taurus . cs .nps .navy.mil/pub/mosaic/auv.html 

echo  'remove  auv  underwater  virtual  world  distribution...' 
echo  'be  sure  you  run  this  from  the  directory  you  installed  it' 
echo  ' (ordinarily  your  home  directory) ' 

-f  auv-uvw.  tar 

rm  -f  auv-uvw.tar .Z 

echo  'warning:  unaliasing  rm...' 
unalias  rm 


rm 

-f  -r  execution/* 

rm 

-f  -r  dynamics/* 

rm 

-f  -r  auv-uvw/* 

rm 

-f  -r  DIS.mcast/* 

#  remove  any  inadvertantly  created  .dot  files 

rm 

“f  -r  execution/.* 

rm 

-f  -r  dynamics/.* 

rm 

-f  -r  auv-uvw/.* 

rm 

-f  -r  DIS.mcast/.* 

rmdir 

execution 

rmdir 

dynamics 

rmdir 

auv-uvw 

rmdir 

DIS .mcast 

rm 

-f 

auv-uvw .viewer 

rm 

-f 

auv . html 

rm 

-f 

mbone . html 

rm 

-f 

np8_mosaic . html 

rm 

-f 

. sd . tcl . auv-uvw 

rm 

-f 

.mail cap . auv-uvw 

rm 

-f 

. cshrc-excerpt . auv-uvw 

rm 

-f 

auv-uvw .virtual -world .README 

rm 

-f 

auv-uvw . virtual -world . INSTALL 

rm 

-f 

auv-uvw .virtual -world .REMOVE 

rm 

-f 

auv-uvw . * 

echo  'restoring  .sd.tcl  with  your  saved  .sd.tcl.old,  please  confirm 

echo  cp  .sd.tcl.old  .sd.tcl 
cp  -Sd.tcl.old  .sd.tcl 

echo  'auv-uvw. virtual-world. REMOVE  complete.' 
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m.  NPS  AUTONOMOUS  UNDERWATER  VEHICLE  EXECUTION  LEVEL 
A.  Introducdon 

The  execution  level  of  robot  software  is  the  hard  real-time  process  which 
controls  propellers,  thrusters,  plane  surfaces,  sonars  and  other  sensors.  Hardware 
interfaces  to  the  robot  microprocessor  are  controlled  by  the  execution  level.  The 
execution  level  is  further  tasked  with  interprocess  communications  with  the  tactical 
and  strategic  levels  of  the  robot  architecture  (Byrnes  93),  as  well  as  the  virtual  world 
when  operating  in  the  laboratory. 

The  execution  code  presented  here  has  been  tested  under  a  wide  variety  of 
laboratory  conditions.  Regrettably  much  of  the  interface  code  to  actual  vehicle 
hardware  has  not  been  tested  since  early  1994  when  a  battery  explosion  caused  major 
damage  to  the  NPS  AUV  II.  Hardware  repairs  from  February  through  October  1994 
were  required  before  in-water  testing  resumed.  Nevertheless  pre-explosion  hardware 
interface  source  code  has  been  retained  in  place  and  clearly  identified  through 
comments  and  compile  flags.  Future  work  includes  verifying  hardware  interface  code 
(Marco  95),  updating  the  execution  code  and  then  running  the  combined  virtual 
world/real  world  version  of  the  execution  level  in  the  water. 

An  important  benefit  of  a  robot  connected  to  an  integrated  simulator 
(Brutzman  92a,  92b,  92c)  or  underwater  virtual  world  (Brutzman  94)  is  that  robot 
software  development  can  continue  independently  of  actual  robot  readiness. 
Approximately  half  of  the  execution  level  source  code  presented  here  was  developed 
during  the  eight  month  period  that  the  NPS  AUV  II  was  out  of  commission.  This 
advantage  alone  proved  to  be  strong  justification  for  the  value  of  providing  a  network 
connection  to  the  actual  robot  microprocessor  and  software.  The  essential 
requirement  for  integrated  simulation  in  an  underwater  robotics  research  and 
development  program  has  been  subsequently  confirmed  by  other  researchers 
(Trimble  94)  (Kuroda  94). 
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Another  important  strength  of  the  robot  software  is  that  the  Kemigan  and  Richie 
variant  C  language  source  code  is  able  to  compile  correctly  under  two  dissimilar 
operating  systems:  SGI  Irix  5.2,  a  Unix  variant,  and  OS-9  version  2.3,  a  real-time 
(grating  system  that  includes  features  similar  to  both  Unix  and  DOS.  Compiler 
directives,  customized  nudcefiles  and  command  line  aliases  are  used  to  provide  this 
cross-platform  portability.  Network  communication  via  sockets  has  also  been  made 
compatible  between  these  two  curating  systems.  This  flexibility  has  been  invaluable 
for  rapid  and  effective  software  development.  One  example  out  of  many  is  that 
diagnosis  and  debugging  tools  under  Unix  are  far  more  robust  than  under  OS-9, 
permitting  detection  and  correction  of  subtle  difficulties  not  detectable  by  the 
OS-9  C  language  compiler  and  linker. 

Future  work  includes  upgrading  execution  and  tactical  level  communication 
links.  Currently  communications  between  levels  use  serial  and  parallel  ports,  a  highly 
customized  and  error-prone  hardware  arrangement.  By  adding  Ethernet  connectivity 
to  the  tactical  level  computer,  all  software  levels  will  be  able  to  communicate  via 
sockets  regardless  of  what  networked  processor  or  workstation  they  might  physically 
reside  on.  Additional  long  term  goals  include  extending  Internet  protocol 
compatibility  to  acoustic  telemetry  communications.  Ultimately  underwater  robots 
will  be  independent  nodes  on  the  Internet  regardless  of  whether  they  are  in  the 
laboratory,  operating  with  a  tether  or  swimming  freely  and  autonomously  within  an 
acoustic  local  area  network  (ALAN).  The  sophistication  of  virtual  worlds  will  grow 
closer  to  that  of  the  real  world  as  robots  and  researchers  operate  in  each. 
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B.  mission,  script.  HELP  Robot  Execution  Level  Mission  Script  Syntax 

// . // 

-/execution/mission. script .HELP  11  October  94 

Mission  script  syntax  for  NPS  AUV  execution  level  control  in 

the  NPS  AUV  Underwater  Virtual  World 
Don  Brutzman  brut zman®nps .navy .mil 

// . - . . . - . - . - . -// 

This  file  describes  how  to  change  and  create  NPS  AUV  mission  script  files. 

Files  and  the  'execution'  program  are  in  the  -/execution  subdirectory. 

To  run  a  new  mission,  copy  an  existing  mission  file  over  file 
'mission. script'  or  edit  the  mission . script  file  for  a  new  mission. 

Example:  unix>  cd  execution 

unix>  cp  mission. script .siggraph  mission . script 
unix>  execution  remotehost  fletch.cs.nps.navy.mil 

Script  commands  are  read  by  AUV  execution  level  (execution . c) 

from  the  "mission . script "  default  file  at  the  start  of  each  mission. 

Some  of  the  following  commands  will  also  work  when  invoked  from  the  command 
line  upon  execution. 

Here  are  script  keywords  (and  synonyms)  that  are  currently  recognized: 


Keywords  Parameters  Description 

Synonyms  [optional]  (units  in  feet  or  degrees  as  appropriate) 


Provide  a  list  of  available  keywords 
(as  specified  in  this  HELP  file) . 


REMOTEHOST  hostname  tells  execution  level  to  open  socket  to  virtual  world 

REMOTE  hostname  which  is  already  executed  and  waiting  on  'hostname' 

REMOTEHOST  is  a  command  line  switch.  Example: 

\inix>  execution  remotehost  fletch.cs.nps.navy.mil 

//  comments  follow  on  this  line  which  are  not  executed 

/*  note  comments  will  still  be  spoken  if  AUDIO-ON 

#  pound  sign  also  indicates  a  comment  if  in  first  column 

WAIT  #  Wait  (or  run)  for  #  seconds  (letting  the  robot 

execute) 

RUN  #  prior  to  reading  from  the  script  file  again 

TIME  #  Wait  (or  run)  until  robot  clock  time  # 

WAITUNTIL  #  (letting  the  robot  execute  its  current  orders) 

PAUSEUNTIL  #  prior  to  reading  from  the  script  file  again 

TIMESTEP  #  change  default  execution  level  time  step  interval 

TIME-STEP  #  from  default  of  0.1  sec  to  #  sec 

PAUSE  temporarily  stop  execution  until  <enter>  is  pressed 


HELP 

/? 
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REALTIME- 

REAL-TIME 


run  execution  level  code  in  real-time 

(busy  wait  at  the  end  of  each  timestep  if  time  remains} 
run  execution  level  code  as  quickly  as  possible 


NOREALTIME 

nc--real’:':me 

NuNREALTIME 
NOWAIT 
NO- WAIT 
NOPAUSE 
NO- PAUSE 

MISSION  filename  Replace  'mission . script '  with  'filename'  and  start 

SCRIPT  filename  the  new  mission 

FILE  filename 

KEYBuARL;  read  script  commands  from  keyboard 

KEYBOARL-ON 


KEYBOARD-OFF  read  script  commands  from  mission . script  file 

NO- KEYBOARD 

^LIT 
ST''*'! 

DONE 
EXIT 
REPEAT 
RESTART 
COMPLETE 
<ecf>  markei 

KILL  same  as  QUIT  but  also  shuts  down  socket  to  virtual  world 

SHUTDOWN  'dynamics'  process. 

K?M  M  [##]  Set  ordered  rpir;  values  to  #  for  both  propellers 

SPEED  #  (##]  [or  independently  set  left  &  right  rpm  values 

PROPS  #  [##]  to  #  and  ##  respectively] 

PPOPELLORS  M  (##]  maximum  propel  lor  speed  is  7  00  rpm  =>  2  ft /sec 

COURSE  M  Set  new  ordered  course  (commanded  yaw  angle) 

HEADING  # 

YAW  # 

TURK  it 

change- COURSE  n 

RUDDER  # 

DEADSTICKRUDDER  [#] 

DEPTH  # 

PLANES  # 

DEADSTICK PLANES  [#] 

THRUSTERS-ON 
THRUSTERS 
THRUSTERON 
THRUSTERSON 

NOTHRUSTER  Disable  vertical  and  lateral  thruster  control 

NOTHRUSTERS 
THRUSTERS-OFF 
THRUSTERSOFF 


Change  ordered  course  by  #  degrees 

(positive  #  to  starboard,  negative  #  to  port) 

Force  rudder  to  remain  at  #  degrees 

Force  rudder  to  remain  at  0  [or  #]  degrees 

Set  new  ordered  depth  (commanded  z) 

Force  planes  to  remain  at  #  degrees 

Force  planes  to  remain  at  0  [or  #]  degrees 

Enable  vertical  and  lateral  thruster  control 


do  not  execute  any  more  commands  in  this  script,  but 
repeat  the  mission  again  if  LOOP-FOREVER  is  set 


ROTATE 


# 


open  loop  lateral  thruster  rotation  control 
at  #  degrees/sec 


LOOP- FILE-BACKUP 


back  up  output  files  during  each  loop  replication 
to  permit  inspection  while  new  files  are  written 
the  backup  files  are  in  execution  directory: 
output . telemetry .previous  &  output , l_second .previous 


EKT-.PCONT.POLCONSTA.UTS  start  a  keyboard  dialog  to  enter 
ENTEP -CONTROL -CONST.ANTS  revised  control  algorithm  coefficients 


SLIDINGMODECOURSE  Sliding  mode  course  control  algorithm  {not  yet  workinq) 

SLI LING-MODE-COURSE 


SLIDINGMOLEOFF  Disable  sliding  mode  course  control  algorithm 

SLIDING-MODE-OFF 


SONARTRACE 

SONARTRACEOFF 

SONAR INSTALLED 

PARALLELPORTTRACE 

AUDIBLE 

AUDIO 

AUDIO- ON 

SOUND-ON 

SOUNDON 

SOUND 


enable  verbose  print  statements  in  execution  sonar  code 
disable  verbose  print  statements  in  execution  sonar  code 
sonar  interface  hardware  cards  are  installed,  use  them 
enable  trace  statements  for  parallel  port  communications 
enable  text-to-speech  audio  output 
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Full  speed  (700  RPM)  port  &  starboard  is  used  if 
AUV  distance  to  WAYPOINT  is  >  #standof f-distance  +10', 
then  slows  to  200  RPM  until  within  #standof f-distance, 
then  HOVER  control. 
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C.  execution.c  Robot  Execution  Level  ReaLTime  Control  Program 


Authors : 

Revi seo : 

S'ystem : 
Compiler : 

Compilation : 

[68C20] 

[68030] 


ilrix  ] 


execution.c  AW  execution  level  program 
Don  Brutzman,  Dave  Marco  &  Walt  Landaker 
28  October  94 

AUV  Gespac  68020/68030,  OS~9  version  2.4 
Gespac  cc  Kernighan  &  Richie  (K&R)  C 

put  execution.c 
auvsiml>  chd  execution 
auvsiml>  make  “k2f  execution 
auvsiml>  make  execution 

fietch>  make  execution 


Pi ct  t i no  : 


Ij^rscnpti  cn ; 


fletch>  cd  execution 

fletch>  execution  remote  dynamics^hostname 

where  dynamics-hostname  is  the  IP  name  of  the  host  runninq 
the  dynamics  {virtual  world)  program 

see  gnuplot  scripts  ' auv_plot .gnu '  and  'auv_pIot_l  second. gnu ^ 
tietcn>  gnuplot  auv_plot.gnu 

gravy ^ : -brutzman/ execution>>  lint  -Im  execution.c 

lint  -Im  -Iglobals.h  -Idefines.h  globals.c  parse  functions  c  \ 
execution.c  ” 

fletch>  make  warnings 

closed  loop  for  operation  during  vehicle  in-water 
missions  as  well  as  in  virtual  world 


MCtive  chanc 


Future  work; 


Don  Brutzman  working  lab/virtual  world  networked  version 
Sc  tactical  level  interface 

Dave  Marco  working  vehicle  code 

&  interfacing  physical  devices 

Update  digital  <==>  analog  access  for  new  vehicle  hardware 

Retest  code  after  vehicle  repaired 

Sonar/altimeter  integration  code  reintegrated/retested 

Audio  strings  seem  to  be  generated  differently  by  OS-9 

standardize  parsing  of  command  line  and  script  commands 

finish  sliding  mode  control 

change  serial /parallel  comms  to  sockets  once 
tactical  level  gets  an  Ethernet  card 


Testing  interprocessor  connections: 
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parallel  port 

/P 

OS-9 

auvsiml> 

mf i„a3 

LPTl  : 

DOS 

auvsim2> 

> 

portf ix 

print  filename.txt 

serial  port 

(/Tl)  /TT 

OS-9 

OS-9 

DOS 

auvsiml> 

auvsiml> 

auvsim2> 

wr2tl  then  write 

rdtla  then  read 

C:\COMM\PROCOMM 

then  <alto>  for  chat 
<altF10>  help,  <altX> 

text 

text 

mode 

exit 

Interfaces : 


Telemetry  sent  via  serial  port  /tt  [==  /tl  at  high  baud  rate] 

Telemetry  received  via  parallel  port  /P 

Telemeti'y  is  optionally  passed  to/from  tactical  level  running  on  80386 

Reads  files:  mission. init  [mission  initialization  data  file  ] 

Writes  files:  output. data  [vehicle  telemetry  state  vector  data] 

output. auv  (tactical  order/executive  report  log] 

Sonar  com.mands / repl ies  via  device  port  /t3 

Note  that  %F  double  formats  are  used  instead  of  %lf  on  scanf  ( )  and  sscanf ( ) 
calls  for  OS*9  compatibility 


^include  "globals . h ” 

^include  "def ines . h " 

/*  function  prototypes  *! 

/'  IS  there  seme  way  to  put  parameter  specifications  in  the  prototypes??  */ 

/’  only  if  we  buy  the  ANSI  C  compiler  from  Microware  (or  shift  to  VxWorks)  *l 


void 

closed_loop_control_module 

0 

void 

get_control_constants 

0 

do^r 1^ 

depth 

0 

double 

calculate_psi 

0 

veld 

2  r  c_gy  r  c_da  t  a 

0 

void 

ini tial ize^dacs 

0 

void 

ini tialize_adcs 

0 

void 

con trcl_sur face 

{) 

VC .  d 

rudder 

0 

VC  1 G 

planes 

0 

void 

ma i n_mo t  o  rs_o  f  f 

{) 

void 

alive 

0 

V  C  I  d 

record_data_on 

0 

void 

record_data_of f 

0 

void 

record_data 

0 

double 

roll_angle 

0 

double 

pitch_angle 

0 

double 

heading 

{) 

double 

roll_rate_gyro 

0 

double 

p  i  t  ch_r  a  t  e_gy  r  0 

0 

double 

y  a  w_r  a  t  e_gy  r  o 

{) 

double 

s  t  bd_mo  1 0  r_rpm 

0 

double 

port_motor_rpm 

{) 

double 

get_speed 

0 

void 

get_init_avg 

{) 

void 

get_avg_rng 

0 

void 

open_device_paths 

0; 
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void 

unsigned  char 
unsigned  char 


close_device_pa ths 

read__parallel_port 

daci 

dac2b 

add 

adc2 

Ini t_PcrcA 
Ini t_PorcB 
Read_PortA 
Read_PortB 


unsigned  short  Read_PortAB 
void  set_bsyA 

void  rst_bsyA 

int  ck  sta 


lie  - 1 
dour  e 


lOUD^tr 


center_sonar  () 
query _sonar_l_reply  ( ) 
set_step_size  {) 
tty^Tuo6e  O 

print_valid_key  words  {) 

open_vi rtuaI_worId_socket  () 
shutdowri_virtual_world_socket  { ) 
3end_data_on_vi rtual_worId_socket  { ) 
get^stream^f  rorr:_virtual_world_sock€t  ( ) 


aegrees 
radians 
normal i ze 
n o iiTia  1 1  2 e 2 

radi an_normai ize 
radian_ncrmali2e2 

c  i  amp 


/*  rroiTi  parse^funct  ions  .  c 

extern  void  parse_connnand_line_f lags 

extern  void  parse_mission__script_comiriands 

extern  void  parse_mission_string_commands 


/*  OS-9  ~  specific  function  compatibility 
*if  defined  (sgi) 


void  tsleep  (unsigned  svalue)  {  /*  null  body  */} 

void  _sysdate  (format,  time,  date,  day,  tick) 

int  format,  *time,  *date,  *tick;  short  *day; 

{  /*  null  body  */} 

double  pow  (xx,  yy)  /*  power  function  */ 

double  XX,  yy; 

{return  exp  (yy  *  log  (xx) ) ; } 

int  _gs_rdy  (path)  int  path;  {  return  0;  )  /*  tytes  waiting  on  path  */ 

int  _gs_opt  (path,  buffer) 
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int  path;  struct  sgbuf  *buffGr; 
{  return  0;  } 

ss_opt  (path,  buffer) 

int  path;  struct  sgbuf  ^buffer; 
{  return  0;  } 


/■>rlr»»»»-»-»***»»***»*****»*******»**'**'»1t-*****»»*ir*1t^'*****************************y/ 


:ai'jc,  argv 

anc  argc;  char  **argv;  /*  command  line  arguments  */ 

{ 

if  {TRACE  &&  DISPLAYSCREEN)  printf  (* | start  main :  execution] \n" ) ; 
r: rcpy  ( v: rtua 2_wcrld_remote_ho3t_name ,  VIRTUAL_WORLD_REMOTE_HOST_NAME) ; 
dt  =  TIMEETEP; 


p3rse_comiriand_Ilne_f lags  (argc,  argv)  ; 

get_contrcl_constants  (); 

initial! ze_dacs  { j ; 
initiaiize_adcs  0; 

open_de V i c e_pa  t n s  ( ) ; 

reccrd_data_on  /*  Open  files  for  data  logging  */ 

oper._vi rtual_wcrld_socket  {};  /*  open  connection  to  virtual  world  */ 

if  (strlen  (buffer)  >0) 

se:-id_data_on_virtual_world_socket  { )  ;  /*  SILENT?  send  to  sound  driver  */ 

strcpy  {buffer,  "  A  U  V  virtual  world  socket  is  open"}; 

send_data_on_vi rtual_world_socket  {);  /*  buffer  containing  message  sent  */ 

»»»-r»*»T»»'r-»-X'*-**w-***»*»'*r***-********»****1t******'*******'**************'*^*****y' 

dc  /*  wnile  (LOCATIONLAE  UU  LOOPFOREVER)  */ 

^  /*  indefinite  repeat  loop  for  long-duration  lab  testing  */ 

--- - - 

if  .LISPLAYSCREEN) 

{ 

if  (LOCATIONLAB  &&  (EMAIL  ==  TRUE)) 

{ 

strcpy  (buffer,  "  Please  Enter  Your  E-mail  Address"); 

send_daca_on_virtual_world_socket  {);  /*  buffer  containing  msg  sent  */ 
printf  ("%s  HERE  ",  buffer); 

strcpy  (email_address,  "•); 
gets  (email^address) ; 

sprintf  (buffer,  "Thanks  %s\n",  email_address) ; 
printf  (•%s\n",  buffer); 

send_data_on_virtual_world_socket  ();  /*  buffer  containing  msg  sent  */ 

if  ((int)  (strlen  (ema Headdress)  >  2)  && 

(strcmp  (email_address,  "brutzman")  ‘=0)  && 

(strcmp  (ema Headdress,  "BRUTZMAN")  !=  0)) 

{ 

emailaddressfile  =  fopen  ( EMA I LADDRESS FILENAME,  "a");  /*  append  */ 

fprintf  (emailaddressfile,  ■%s\n",  email_address) ; 
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fclose  (emailaddressf ile) ; 

i 

/ 

eli^e  if  .LOCATIONLAE  ==  FALSE) 
i 

aaive  (10,  start_dwGl 1 ) ;  /*  Wag  fin  every  10  seconds  for  total  duration 

of  start_dwell  seconds  */ 

printfl"  Position  AUV  for  Directional  Gyro  Offset  MeasureinentVn" )  ; 
printf("  Rate  Gyro  zero  iDeasurementXn* )  ; 

printf(’'  Hit  <Enter>  on  AUV  When  Ready  ***  Here  !  ***\n"); 
answer  =  getchar  {);  /*  pause  */ 

^prantfl"  OK!!  Starting  the  mission . \n ") ; 

parse_mission_script_coininands  ();  /*  read  initial  script  orders  */ 


2erc_gyrG_data  ( )  ; 
if  (SONARINSTALLED)  center 


Get  daily  zeros  for  gyros  */ 

_sonar  ();  /*  must  have  open_device_paths  1st  */ 


scrcpy  (buffer,  "  /»  pause  */ 

str-ijd_daca_on_virtual_world_sockei:  ();  /*  buffer  containing  msg  sent  */ 

surcpy  'buffer,  "  AUV  is  starting”); 

senQ_dat5_on_virtual_wor ld_socket  0;  /*  buffer  containing  msg  sent  *! 

. I n:  t  i a  1  i  za  t i on  of  closed  loop  parameters 

bu  f  f  er_inde>:  =  0  ; 

tea emet  ly  _records_saved  =  0; 

iTii3sion_ieg_counter  =  0; 

end^test  =  FALSE; 

wrap^count  =  0; 

‘  =  C  .  0  ; 

/  y _ _ _ _ _ _ _ 

/’  Main  program  operational  loop  code 
it  (TRACE  S.S,  DISPLAYSCREEN) 

print f  (” (Starting  main  program  operational  loop  code...  ]■); 
previousloopclock  =  clock  (); 

- - 

while  (end_test  ==  FALSE)  /*  this  is  the  realtime  main  operational  loop 
^  /*  when  end_test  ==  TRUE  then  loop  is  done 

closed_loop_control_module  ( ) ;  /*  closed  loop  code  is  here  ««««« 
P  /*  end  of  real-time  main  operational  loop 

/*  lab  version  may  repeat  forever  for  long-duration  testing- 
replication_count  **; 

if  (LOCATIONLAE  &&  LOOPFOREVER  &&  DISPLAYSCREEN) 

printf  (*\n[LOOP  FOREVER  enabled,  next  loop  is  replication  %d  l\n' 
replication_count) ; 

sprintf  (buffer,  •  LOOP  FOREVER  enabled,  next  loop  is  replication’) • 
send_data_on_virtual_world_soc)cec  ();  /*  buffer  msg  sent  */ 

sprintf  (buffer,  *  %d‘,  replication_count ) ; 
send_data_on_virtual_world_socket  (),-  /*  buffer  msg  sent  */ 


if  (LOCATIONLAE  &&  LOOPFOREVER) 
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/■*■  reset  ainount  of  time  to  wait  for  next  command  i 
t ime^next^command  =  C . 0 ; 
t  =0.0; 

if  (DISPLAYSCREEN) 

{ 

print f  ( * XnLoopforever  reset  time:  [ time_nGxt_command  =0.0]  ■); 
print f  (-  [t  =  0.0]\n-) ; 

} 


if  (LOOPFILEBACKUP) 


record_data_of f  ( )  ; 


def 


se 


f 


ned (sgi ) 


printf  •' 

"  rm 

system  ( 

"  rm 

printf  ( 

"cp 

mission , output . telemetry 

system  ( 

*cp 

mission . output . telemetry 

printf  ( 

“  rm 

system  ( 

"  rm 

printf  ( 

"  cp 

mission . output . l_second 

system  ( 

“cp 

mission . output . l_second 

printf  ( 

"del 

system:  ( 

"  del 

printf  ( 

-copy 

mission . output . telemetry 

system!  ' 

"  copy 

mission . output . telemetry 

printf  ( 

"del 

system^  ( 

“del 

printf  { 

"copy 

mission . output . l_second 

system  ; 

“  copy 

mission . output . l_second 

I f  1 LOCA 

iT01>iLAB; 

{ 


ou tput . telemetry  .previ ousXn" ) 
output . telemetry  .previous*  ) 
output . telemetry .previ ousXn* ) 
output . telemetry .previous "  ) 
output . l_second. previous Xn"  ) 
output . l_second. previous"  ) 
output . l_second. previous Xn"  ) 
output .l_second. previous"  ) 

output . telemetry  .previ ousXn" ) 
output . telemetry .previous"  ) 
output .telemetry .previ ousXn" ) 
output . telemetry  .previous "  ) 
output . l_second. previous Xn"  ) 
output . l_second, previous "  ) 
output . l_second. previous Xn"  ) 
output .l_second. previous"  ) 


strcpy  (buffer,  "  telemetry  data  backup  complete"); 
send__data_on_virtual_world_socket  ();  /*  buffer  msg  sent  */ 


tlse  /*  don't  bother  backing  up  most  recent  results 


if  (LOCATIONLAB  LOOPFOREVER) 

rewind  (auvtext f i le } ; 
rewind  (auvdatafile) ; 
if  (TRACE  &&  DISPloAYSCREEN) 

printf  ( "  lauvtextf ile  h.  auvdatafile  rewound  to  "); 
print f  ( "output .data .previous  &  output . auv. previous ] Xn" ) ; 
strcpy  (buffer,  "  telemetry  data  backup  skipped"); 
send_data_on_virtual_world_socket  {);  /*  buffer  msg  sent 

} 

) 


f flush  (auvscriptfile) ; 
if  (fclose  (auvscriptfile)  ==  0) 

{ 

if  (DISPLAYSCREEN) 

printf  (• [success  closing  auvscriptfile  mission. script .backup] Xn* ) ; 
else  if  (DISPLAYSCREEN) 

printf  ("[failure  closing  auvscriptfile  mission. script .backup] Xn" ) ; 

f flush  (auvordersf i le) ; 
fclose  (auvordersf ile) ; 


/• 

#if  defined (sgi) 
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#el  se 


#endif 

•/ 


if  (TP.UE  &&  DISPLAYSCREEN) 
pr:nrf  •"nr 
system  { "  nr 

printf  f"mv  mission .output 
system  ("mv  mission . output 

printf  (“del 
system  (“del 

printf  (“copy  mission .output . 
system  (“copy  mission . output . 

printf  (“del  mission .output . 
system  {"del  mission .output . 


mission . output . orders\n“ 
mission . output . orders " 

.  orders .backup  mission . output . orders\n“ 

.  orders . backup  mission . output . orders “ 

mission. output .orders\n" 
mission. output .orders"  ) ; 
,  orde rs . backup  mi ss i on . ou tpu t . o rder s \n “ ) ; 
, orders .backup  mission. output .orders “  ); 

.  orders .backup\n\n“ ) ; 

,  orders .backup"  ); 


orders 

#if  defined(sgi) 

sprintf  (buffer,  “rm%s\n",  AUVORDERSFILENAME) ; 

*e]  se 

rrv:r.:f  (buffer,  “de:  %s\n",  AUVORDERSFILENAME); 

#e!-d.f 

:f  (DISFLAYSCREENi  printf  {“%s\n",  buffer); 
system  -buffer/; 


#if  defined (sgi ) 


sprintf 

^eise 

sprintf 

(buffer. 

“mv  %s. backup  %s\n", 

AUVORDERSFILENAME , 

AUVORDERSFILENAME) 

{buffer. 

"copy  %s. backup  %s\n". 

AUVORDERSFILENAME , 

AUVORDERSFILENAME) 

*»eridi  f 

if  ^riEPLAYSCREEN:  printf  ("%s\n",  buffer); 
system  (buffer) ; 

r  defined isgi ] 

^  e  1  se 

sprintf  (buffer,  "del  %s  \n",  AUVORDERSFILENAME); 

if  (DIEPLAYSCREEK;  printf  (“%s\n",  buffer); 
system  (buffer); 

#end:f 


-  * 

^if  def  iiied  (sgi  ) 

sprintf  (buffer,  “rm  %s\n", 

^  e^  se 

-  e-mail 

AUVEMAILFILENAME) ; 

'/iintf  (buffer,  “del  %s\n", 

AUVEMAILFILENAME)  ; 

if  'DISPLAYSCREEN)  printf 
system:  (buffer); 

#:f  deflned(sgi) 

( "%s\n" , 

buffer) ; 

sprintf  (buffer,  "cp  %s 

#else 

%s\n" , 

AUVINFOFILENAME, 

AUVEMAILFILENAME) ; 

sprintf  (buffer,  “copy  %s 

#endi  f 

%s\n“ , 

AUVINFOFILENAME, 

AUVEMAILFILENAME) ; 

if  (DISPLAYSCREEN)  printf 
system  (buffer); 

(*'%s\n-. 

buffer) ; 

#if  defined{sgi) 

sprintf  (buffer,  “cat  %s  »  %s\n“,  AUVSCRIPTFILENAME,  AUVEMAILFILENAME) ; 

#else 

sprintf  (buffer,  “list  %s  »  %s\n",  AUVSCRIPTFILENAME,  AUVEMAILFILENAME); 

#endi f 

if  (DISPLAYSCREEN)  printf  (“%s\n“,  buffer); 
system  (buffer) ; 
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definedisgi) 

sprintf  (buffer,  “cat  %s  >>  %s\n“,  AUVORDERS FILENAME,  AUVEMAILFILENAME) ; 

^  e  I  ?e 

sprintf  {buffer,  "list  %s  >>  %s\n",  AUVORDERS FILENAME,  AUVEMAILFILENAME); 

^endlf 

:f  (DISFLAYSCREEN)  printf  (“%s\n",  buffer); 
systeni  (buffer); 

if  ((int)  (strlen  (email^address)  >=  3)  &&  (EMAIL  ==  TRUE)) 

{ 

sprintf  (buffer,  “mail  %s  <  %s",  ema Headdress,  AUVEMAILFILENAME); 
*:f  defined (sgi ; 

printf  {“%s\n“,  buffer); 
system  (buffer) ; 

^  e  1  se 

/*  system  (buffer);  /*  e-mail  not  available  directly  on  OS-9  */ 

send_data_on_virtual_world_socket  ();  /*  buffer  msg  sent  anyway  */ 

#endi f 

} 

/*  pei-mit  changing  the  vehicle  mission  during  continuous  lab  testing  */ 
if  (LOCATIONLAE  LOOPFOREVER) 
i 

get_control_constants  ( ) ; 
previouslcopclock  =  clock  (); 
record_data_on  { ) ; 

strcp\’  (buffer,  "  Load  mission  again"); 
send_data_on_virtual_world_socket  () ; 

/*  buffer  containing  message  sent  */ 

}  wh::e  (LOCATIONLAB  &&  LOOPFOREVER);  /*  end  of  lab  infinite  loop  (if  any)  */ 

rr- ::-._n.c  c  cr.?_cf  f  ();  /  *  all  done,  turn  them:  off  */ 

:f  (TRACE  DISFLAYSCREEN) 

printf  '“i sending  'shutdown'  message  to  virtual  world  dynamics] \n“ ) ; 

strcpy  (buffer,  “shutdown");  /*  must  start  with  'shutdown'  to  die  */ 

s-ndlda: rtual_world_socket  ();  /*  buffer  containing  message  sent  */ 

shutdown_virtual_worid_socket  (;;  /*  close  connection  to  virtual  world  */ 

close_device_paths  ( ) ; 

record_data_of f  {); 

if  (TRACE  S.&  DISFLAYSCREEN) 

printf  ("[ finishing  main :  fflush  (stdout) ,  fflush  (stderr ) ] \n* ) ; 

f flush  (stdout); 
fflush  (stderr); 

if  (TRACE  &&  DISFLAYSCREEN)  printf  ("[main  exit:  return  (0)]\n"); 
defined  (sgi) 

if  (DISFLAYSCREEN)  printf  ("gnuplot  auv_plot_l„second. gnu\n" ) ; 
system  (“gnuplot  auvjlot_l_second  .gnu " ) ;  /*  display  plotted  results  ♦/ 
tendif 

return  (0);  /*  main  program  exit  */ 

)  /*  end  main  program  block,  execution  is  complete  */ 
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********-****^ 


*»*★★»***** 


★  ★*★********•***»**★**♦**** 


»  *  *  y 


void  closed_ioop_control_module  {) 

if  (TRACE  &&  DISPLAYSCREEN) 

printf  {“[start  closed_loop_control_module] Xn" ) ; 

Speed  Control  ************'******»****************»*»****************»**y 

speed  =  get_speed  (); 

rprr,  =  (porc_rpm_coinmand  +  stbd_rpm_comitand)  /  2.0; 

clamp  (&£  rpm,  700.0,  -700.0,  “rpm’*);  /*  bound  maximum  RPM  */ 

..TRACE  LTSPLAYSCREEN) 

printf  (“[clamp  (&  rpm,  700.0,  -700.0,  \*'rpm\")  complete]  \n" )  ; 

/ ’■  Main_Motor  RPM  Control  *****-*'*»**»***»^*****<^****»****-****it*******»nfit**/ 

/*  note  thruster  use  does  not  preclude  propeller  use  */ 

if  ;LOCATICNLAB  ==  TRUE) 

{ 

pcrt_rpm  =  port_rpm_comjTiand; 
s  t  Dd_rpn  =  St  bd_rpn;_c  omma  nd  ; 

else  /*  in  water,  propeller  control  */ 

pcrt_rpm:  =  port_motor_rpm  ( )  ; 
stbd_rpm  =  stbd_motor__rpir:  (); 


1 f  iNCT^YET^REIMPUEMENTED) 

{ 

main_iT:Ctor_dei tal  =  fabs(rpmi)  -  stbd_rpm; 
main_mtOtcr_del  ta2  =  fabs{rpm.)  -  port_rpm; 

/  ^his  IS  reset  windup  for  proportional  integral  control  of  motor  speed  */ 
/ *  in  order  to  prevent  accumulating  the  integral  of  speed  error  */ 

if  (main_motor_del tal>50 . 0 )  main_motor_del tal  =  SO.O; 
if  ^rT,ain_rriOtor_del  ta2>50 . 0  )  main_motcr_del  ta2  =  50.0; 

-Tiair._motor_vol  1 1  =  main__motor_vol tl  +  (rpm/fabs  (rpm)  *0 . 2*main_motor_del  tal )  ; 
main_motor_vol t2  =  main__motor_vol t2  + ( rpm/f abs (rpm) *0 . 2*main_motor_delta2 ) ; 

if  (main_motor_voj tl  >  1023)  main_motor_volt 1  =  1023; 

■  n,ain_motor_vci  tl  <  0)  main_motor_vol tl  =  0; 

if  ■: main_motor_voi  t2  >  1023)  main_motor_vol t2  =  1023; 

imain^nictor^vol  t2  <  0/  main_motor_vol  t2  =  0; 

dad  (mairi_motor_voltl,RIGHT_MOTOR)  ; 
dad {main_motor_volt2,LEFT_MOTOR} ; 

/'  if  using  virtual  world  dynamics,  network  is  source  of  values  ««««  */ 

phi  =  roll_angle  ();  /*  read  roll  angle  */ 

theta  =  pitch_angle  ();  /*  read  pitch  angle  »/ 

psi  =  calculate^psi  ();  /*  Read  heading  ♦/ 

p  =  roll_rate_gyro  (};  /*  read  roll  rate  */ 

q  =  pitch_rate_gyro  ();  /*  read  pitch  rate  */ 

r  =  yaw_rate_gyro  {);  /*  Read  yaw  rate  */ 

2  =  depth  0;  /*  Read  depth  ♦/ 

/*  note:  in  laboratory  using  virtual  world,  values  above  are  superceded  */ 


32 


/’  Control  laws  **”  NOTE:  all  k_  constants  must  be  (+)  positive  *•**  */ 

waypcint_distance  =  sqrt  (  (x  -  x_coinmand)  *  (x  -  x_coiranand) 

+  (y  “  y^cominand)  *  (y  -  y_commana) )  ; 

/-  use  WAYPOINTCONTROL  (not  HOVERCONTROL)  until  within  standof f^distance  */ 
if  ((HOVERCONTROL  ==  TRUE)  &&  (waypoint_distance  >  standoff_distance) ) 

{ 

WAYPOINTCONTROL  =  TRUE; 

DEADSTICKRUDDER  =  FALSE; 

if  (waypoint_di stance  >  standof f^distance  +  10.0} 


%6.1f 
%E-.  If\n". 


port_rpm_coiTuriand  =  700; 
stbd_rpm_comiBand  =  700; 
fprintf  (auvordersf ile, 
%5.1f  %5.1f  %5.1f  %6.1f 


%6.1f  %6.1f  %6,lf 


%5.1f  %5.1f  %5.1f 


t,  psi_command,  x^conunand;  y^coirunand,  z_command, 
port_rpin_coiTiinand,  stbd_rpin_coiTiinand, 
rudder^command ,  planes^cominand, 

bow_lateral_thruster_coiTmiand, 
sternal  a  teral_thruster_coininand, 
bow_vertical_thruster_coiT\inand , 
stern_vertical_thruster_cojTunand)  ; 


port_rpni_coiTiinand  =  200; 
stbd_rpir:_coiTUT.and  =  200; 
fprintf  (auvordersf ile, 

^c6.1f  %5-if  %5.1f  %S.lf  %6.1f  %6.1f  %6.1f  %6.1f  %S.lf  %5.1f  %5.1f 
s  -  .  1  f  \  r.  , 

t,  psi_conimand,  x_command,  y„comn\and,  2_coiranand, 
port  _r  prTi_c  arm  and,  s  t  bd_r  piTi_c  oinm  and, 
rudder^coimnand,  planes_coTnrriand, 
bow_l a t e ra l_t h ru s t er_c ommand , 
sternal a teral_thruster_command, 
bow_vertical_thruster_coininand, 
stern_vertical_thruster_coininand)  ; 

} 

else  if  ({HOVERCONTROL  ==  TRUE)  &&  (WAYPOINTCONTROL  ==  TRUE)) 

/’’  restore  proper  HOVERCONTROL  */ 

{ 

WAYPOINTCONTROL  =  FALSE; 

DEADSTICKRUDDER  =  TRUE; 
rudder_command  =  0.(D; 
psi__coTniTiand  =  psi_coiTimand_hover; 

pcrt^rpiTi^cominand  =  0; 
stbd_rpin_command  =  0; 

fprintf  (auvordersf ile, 

•%6.1f  %6.1f  %5.1f  %5.1f  %5,lf  %6.1f  %6.1f  %6.1f  %6.1f  %5.1f  %5.1f  %5.1f 

%_>.lf\n  ,  psi^command,  x^coiratand,  y_coiTunand,  2_command, 

po r t_rpm_c oinmand ,  s t bd_rpm_c ommand , 
rudder^coirmiand,  pi  anes_c  ommand, 

bow_lateral_thruster_command, 
stern_lateral_thruster_command, 
bow_vertical_thruster_command, 
stern_vertical_thruster_command)  ; 


if  (WAYPOINTCONTROL  ==  TRUE) 

^  /*  note  that  a  reversed  x,y  calling  sequence  is  necessary  */ 

/*  in  order  to  get  correct  quadrant  alignment  */ 

waypoint_angle=normali2e (degrees (atan2  (y_command  -  y,  x^command  -  x) ) ) 


psi_cormnand  =  waypoint_angle  ; 

if  (TRACE) 

{ 

printf  { “WAYPOINTCONTROL  psi^comir.and  =  %5.1f,  “ ,  psi^command)  ; 

printf  {**x  =  y  =  %5.1f\n\  x,  y)  ; 

1  • ' 

/ 

if  { (FOLLOWWAYPOI NTMODE  ==  TRUE)  (HOVERCONTROL  ==  FALSE)  && 

( {  waypoint_distancG  >  standof f^distance)  I  I 
{fabs  (  2  -  z_command)  >  scandof f_distance) ) ) 

{ 

if  (TRACE)  printf  { M FOLLOWWAYPOINTMODE  check] -) ; 

I*  continue  until  WAYPOINT  reached  without  further  script  orders  */ 
time_nGXt_cominand  =  t  +  2.0  *  dt; 

)_ 

else  /*  WAYPOINT  reached  */ 

( 

if  (TRACE)  printf  (**  1  FOLLOWWAYPOINTMODE  success,  WAYPOINT  reached]"); 
fprintf  ( auvordersf ile, 

If  li.lt  %5.1f  %5.1f  %5,lf  %6.1f  %6.1f  %6.1f  %6.1f  %5.1f  %5.1f  %5.1f 

I  \  n " , 

t,  psi_comrr.and,  x^coiranand,  y_command,  z_coininand, 
port_rpm_coiTimand ,  stbd_rpni_coinmand, 
rudder^command,  planes_command, 
b  o  w_  1  a  t  e  r  a  1  _  t  h  r  u  s  t  e  r  _c  oiniTi  a  n  d , 
sterri_lateraI_thruster_coiTiinand, 
b  o  V  e  r  t  i  c  a  1  _  t  h  ru  s  t  e  r  _  c  omit  a  n  d , 
stern_verticaI_thruster_command)  ; 

) 

} 

else  if  (HOVERCONTROL  ==  TRUE) 

( 

waypcint_arigle=ncrmalize  (degrees  (atan2  (y_coitmand  ”  y,  x_command  -  x) )  )  ; 
track_angle  =  normalize  (waypoint^angle  -  psi ) ; 

aicng„tracK_drstance  =  cos  (radians  ( track_angle )  )  waypoint_distance; 
cross_track_distance  =  -  sin  (radians (track_angle ) )  *  waypoint_di stance; 

pcrt_rpm  =  k_propel lGr_hcver  *  al ong_track_distance  - 
k_surge_hover  *  u; 

stbd_rprr'  =  k_propei lGr__hover  *  aiorig_track_distancG  - 
k_surge_hover  *  u; 

:f  fTRUE) 

p  r : n  t  f  ( " HOVERCONTROL : \ n “ ) ; 

printf  ( "psi^comiTiand  =  %5,lf,  ",  psi_command)  ; 

printf  i  ”  X  =  %  5 , 1  f  ,  y  =  %  5 . 1  f  \  n  ,  x ,  y )  ; 

printf  ( *'waypoint_distance  =  %5.1f,  track_angle  =  %5.1f\n*, 
waypoint_di stance,  track_angle) ; 

printf  ( "aIong_track_di stance  =  %5.1f,  ",  aIong_track_distance) ; 
printf  ( ‘■cross_track_distance  =  %5.1f\n",  cross_track_distance)  ; 
printf  ("port_rpm  /  stbd_rpm  =  %5.1f\n",  port_rpm) ; 

) 

) 

/*  Simplified  PD  rudders/planes  control  rules: 
delta_rudder  =  k_psi  ♦  nonnalize2  (psi  -  psi_command) 

+  (k_r  *  r)  +  (k_v  ♦  v) ; 

/*  tanh  not  provided  under  OS-9  C,  added  at  end  of  this  program  */ 

if  (SLIDINGMODECOURSE  ==  TRUE) 

{ 

sigma  =  k_sigma_r  *  r  +  k_sigma_psi  *  normaliZG2  (psi  -  psi_command) ; 
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delta_rudder  =  f3.1403  *  r)  +  81.9712  *  eta_steering  *  tanh  (sigma); 

} 

depth^error  =  (z  -  z^command) ; 

/’  constrain  depth_error  to  +-  15.0  feet  to  prevent  going  vertical  */ 

/*  and  enable  stable  pitch  angle  even'  on  large  depth  changes  */ 

clamp  (&  depth_error,  -15.0,  15.0,  “depth^error" ) ;  /*  feet  */ 

delta_planes  =  -  (k_z  *  depth_error) 

+  (k_theta  *  theta)  +  (k_q  *  q)  -  (k_w  *  w) ; 

/*  Dead  stick  means  no  open  loop  control  of  rudders/planes  - 

i f  (DEADSTICKRUDDER ) 

{ 

delta_rudder  =  rudder_command; 

} 

if  (DEADETICKPLANES) 

delta_planes  =  planes^comrriand; 

} 

I"*  constrain  planes  &  rudder  orders  +/-  40.0  degrees  ♦/ 

clamp  (&  del ta_rudder ,  -40.0,  40.0,  “del ta^rudder " ) ;  /*  degrees  */ 

clamp  {5.-  del  ta_planes ,  -40.0,  40.0,  "del  ta_planes  *■ )  ;  /*  degrees  */ 

Send  commands  to  rudders  and  planes  *»********^**»*********************/ 

rudder  (del ta_rudder } ; 
planes  {del ta_planes ; ; 

/*  Simplified  la  teral /vert  ical  thruster  control  rules:  - 

li  (ROTATECONTROL  ==  TRUE)  /*  open  loop  rotate  thrusters  ♦/ 

; 

lateral_thruster_volts  =  k_thruster_rotate  *  rotate_command 

*  rotate_command; 

j 

else  if  (LATERALCONTROL  ==  TRUE)  /*  open  loop  lateral  thrusters  */ 

{ 

lateral_thruster_volts  =  -  k_thruster_lateral  *  (lateral_coinmand) ; 

) 

else  /»  heading  control  is  default  */ 

i 

lateral_thruster_volts  =  -  k_thruster_psi  *  normali2e2  (psi-psi_command) 

-  k_thruster_r  *  r; 

} 

vertical_thruster_volts  =  -  k_thruster_z  *  (z  -  z_coinmand) 

-  k_thruster_w  *  w; 

if  { (THRUSTERCONTROL)  !1  (HOVERCONTROL  ==  TRUE)  II  (ROTATECONTROL  ==  TRUE) ) 

{ 

AUV_bow_vertical  =  vertical_thrust€r_volts; 

AUV_stern_vertical  =  vertical_thruster_volts; 

if  {LATERALCONTROL  ==  TRUE) 

{ 

AUV_bow_lateral  =  lateral_thruster_volts;  /*  both  positive,  */ 
AUV_stern_lateral  =  lateral_thruster_volts;  /*  same  direction  */ 

} 
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else  if  (HOVERCONTROL  ==  TRUE 
{ 

AUT_bow_Iateral  =  -  (  -  : 


AUT_bow_Iateral  =  -  (  -  k_thruster_psi *norTnaIi2e2  (psi -psi^coimnand) 

-  k_thruster_r  *  r) 

+  k_thruster_hover  .*  cross_track_distance 

+  k_sway_hover  *  v; 

AUV_srern_Iateral  =  (  -  k_thruster_psi*nonnali ze2  (psi-psi_coininand) 

-  k_thruster_r  *  r) 
k_thruscer_hover  *  cross_track_di stance 

^  +  k_sway_hover  *  v; 

else  if  ( (THRUSTERCONTROL  ==  TRUE)  II  (ROTATECONTROL  ==  TRUE)) 


A  W__bow_l  a  t  e  r  a  1 
AUV_stern  lateral 


lateral_thruster_volts;  /*  negative  */ 
lateral_thruster_volts; 


printf  ("Thruster  control  logic  error  ***  Xn"); 


if  (TRACE  BISPLAYSCREEN; 

{ 

printf  (  "Thruster  control  ON.  Pre-clamp  calculated  values-\n")* 
prints  AUV_bow_vertical  =  %6.3f,  AUV_stern_vertical  =  %6.3f\n", 

AU\^_bow_vertical ,  AUV_stern_vertical )  ; 

printf  {*■  AUy_bow_lateral  =  %6.3f,  AUV_stern_lat€ral  =  %6.3f\n", 

ACP'-Lbow^iateral ,  AU\^_stern_lateral )  ; 

else  /*  thrusters  disabled  */ 
if  (TRACE  BISPLAYSCREEN; 

printr  (  "Tnruster  control  OFF,  Pre-clamp  calculated  values:\n"}* 
printf  l "vertical_thruster_vci ts  =  %6.3f\n", 
vertical_thruster_volts) ; 
printf  (  "lateral_thx'uster_volts  =  %6.3f\n", 
lateral_thruster_volts)  ; 

ACv^bo  w_ve  rtica]=0.0; 

ALry_sterri_vertical  =  0.0; 

AU"v'_bow_lateral  =  0.0; 

ALPr_3tern_iateral  =  0.0; 

I 

if  (TRACE  IcU  DISPLAYSCREEN) 

{ 

printf  CPre-sqrt  thruster  control  calculated  values • \n* ) • 
printf  { ■AUV_bow_vertical  =  %6.3f\n',  AUV_bow_vertical ) • 
printf  { •AUV_stern_vertical  =  %6.3f\n*,  AUV_stern_verticai ) • 
printf  ("AUV_bow_lateral  =  %6.3f\n*,  AUV_bow_lateral) • 
printf  ( ■AUV_stern_lateral  =  %6.3f\n’,  AUV_stern_lateral) ; 


/*  convert  to  signed  sqrt  to  account  for  volts -to- thrust  relationship 

AUV_bow_vertical  =  2.0  *  sqrt (6.0)  *  sign  (AUV_bow_vertical  ) 

*  sqrt  (fabs  (AUV_bow_vertical  )); 
AUV_stern_vertical  =  2.0  *  sqrt(6.0)  *  sign  (AUV_stern_vertical) 
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*  sqrt  {fabs  (AW_sterrj_verticaI)); 

AU\^_bow_l3teral  =  2.0  *  sqrt  (6.0)  *  sign  {AUV_bow_lateral  ) 

*  sqrt  (fabs  {AUV_bow_lateral  )); 
-  i'r._laterai  =  2.0  *  sqrt  (6.0)  *  sign  (AUV_stern_lateraI  ) 

*  sqrt  (fabs  {AUV_stern_lateral  )); 

if  (TRACE  DISPLAYSCREEN) 

{ 

printf  ("Post-sqrt  thruster  control  calculated  values : Xn* ) ; 
printf  ( "AUV_bow_vertical  =  %6.3f\n"/  AUV_bow_vertical ) ; 
printf  ( "AUV^stern^vertical  =  %6.3f\n*,  AUV.stern.vertical ) ; 
printf  ( *AlJV_bow_lateral  =  %6.3f\n'',  AUV_bow_lateral)  ; 
printf  ( -AUV.sternJateral  =  %6.3f\n-,  AUV_stern_lateral )  ; 

) 


/*  constrain  thruster  orders  +/-  24.0  volts  ==  3820  rpm  no-load  */ 

clamp  (&  AUV_bow_vert ical ,  -24.0,  24.0,  "AUV_bow_vertical " ) ; 

cla.mp  (it  AIJV_stern_vertical ,  -24.0,  24.0,  "AUV^stern^verticai  “ )  ; 

•tlamp  AUV_bow_lateral ,  -2  4.0,  24.0,  •AUV_bow_lateral  * )  ; 

clamp  (&  AUV_stern_lateral,  -24.0,  24.0,  "AUV_stern_lateral - ) ; 

if  {  (port_rpn-_command  ==  0.0)  &&  { stbd_rpm_coininand  ==  0.0)) 

/*  prevent  planes  chatter  by  zeroing  them  at  zero  speed  */ 

t  a__rudQei  =  0.0; 

delta_planes  =  0.0; 


/*  Normalization  within  bounds 

delta_rudder  =  normalize!  (delta_rudder); 
dtrlta^fjlanes  =  normalize!  (delta^planes)  ; 

constrain  planes  &  rudder  orders  +-  4  0.0  degrees  */ 


i:  '.fobs  ( del ta__ru doer ; 

o 

o 

A 

degrees  */)) 

del ta_rudder 

) 

if  (fabs  (del ta^planes } 

i 

=  (40.0)  * 

(delta_rudder  / 

fabs 

(del ta_rudder) ) ; 

>  (40.0  /* 

degrees  */ ) ) 

del ta_planes 

=  (40.0)  * 

(delta_planes  / 

fabs 

(delta_planes) ) ; 

} 


/*  Send  commands  to  rudders  and  planes  **********’^**********^*************/ 

rudder  (del ta_ru Oder ) ; 
planes  ( del ta_pl anes ) ; 

/»  Send  comiTiand  &  get  reply  from  sonar  *»**********♦********♦★**♦*****»♦**/ 

ACV_£7l000_bearing  =0.0;  /*  relative  bearings  of  sonar  heads  */ 

AUV_£T725_bearing  =  0.0; 


/*  send  telemetry  to  tactical  level  and  data  recording  files 
record_data  (); 

/*  read  commands  from  tactical  level  - 
if  (TACTICAL  ==  TRUE)  read_parallel_port  (); 


/*  update  simulation  clock  "t" 

t  =  t  +  dt; 

fflush  (stdout); 
currentloopclock  =  clock  (); 
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If  ((REALTIME  ==  TRUE)  &&  LOCATIONLAE  && 

(currencloopclock  <  previousloopclock 

+  {inc}(dt  *  (float)  CLOCKS_PER_SEC) ) ) 

{ 

if  (TRACE  DISPLAYSCREEN) 

{ 

printf  ( “currentloopclock  =  %ld,  previousloopclock  =  %l(d\n“, 
currentloopclock,  previousloopclock) ; 

printf  ( ’‘tiinestep  dt  =  %5.3f  seconds  (corresponding  clock  ticks  =  %d)\n", 
dt,  (int)  (dt  *  (float)  CLOCKS„PER_SEC) ) ; 
printf  ("Busy  wait  until  system  clock  reaches  simulation  clock,  •); 
printf  ("loop  duration  =  %5.3f\n", 

((float)  currentloopclock  -  (float)  previousloopclock) 

/  CLOCKS_PER_SEC) ; 

} 

while  (currentloopclock  <  previousloopclock 

4  (int)(dt  *  (float)  CLOCKS_PER„SEC) ) 

{ 

currentloopclock  =  clock  ();  /*  %%%%%  busy  wait  %%%%%  */ 

} 

if  (TRACE  &&  DISPLAYSCREEN) 

{ 

printf  ("Busy  wait  complete,  loop+wait  duration  =  %5.3f,  ", 

(  (float)  currentloopclock  -  (float)  previousloopclock) 

/  CLOCKS_PER_SEC) ; 

printf  ("current  clock  ()  =  %ld\n",  currentloopclock); 

} 

else  if  ((REALTIME  ==  FALSE)  ScU  LOCATIONLAE  &&  DISPLAYSCREEN  &&  TRACE) 

printf  ("No  busy  wait,  loop  duration  =  %5.3f,  ", 

(  (float)  currentloopclock  -  (float)  previousloopclock) 

/  CLOCKS_PER_SEC) ; 

printf  ("current  clock  ()  =  %ld\n",  currentloopclock); 

} 

previcusloopcl ock  =  clock  (); 

/’  estimate  X  and  Y  by  dead  reckoning 

X  =  X  ^  (speed  *  dt  *  cos  (radians  (psi))); 
y  =  y  *  (speed  dt  *  sin  (radians  (psi))); 

if  i feof  (auvscript f ile)  &&  (t  >=  time_next_command) )  /*  all  done  */ 

if  (TRUE  DISPLAYSCREEN)  printf  ( "end^test  set  TRUE\n"); 
end_test  =  TRUE; 

) 

else  if  (t  >=  time_next_command)  /*  scriptfile  not  yet  closed,  read  more  */ 
( 

if  (TRUE  &&  DISPLAYSCREEN) 

printf  ("\n(read  more  from  parse_mission_script_commands]\n"}; 
parse_mission_script_commands  ();  /*  get  next  script  orders  read  */ 

} 

if  (TRACE  &&  DISPLAYSCREEN) 

printf  ("[finish  closed_loop_control_module  ()]\n"); 


}  /*  end  closed_loop_control_module  ()  */ 


void  get_control_constants 


if  (TRACE  &&  DISPLAYSCREEN) 


/*  get  data  from  file  at  program  start  */ 
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print f  {"[start  get_control„constants  ()]\n"); 

if  (ENTERCONTROLCONSTANTS) 

{ 

printf (" Input  start_dwel} \n" ) ; 
scarif{"%d",  &start_dwell )  ; 

/*  note  %F  required  by  OS-9,  accepted  by  SGI  */ 
printf ("Input  k_psi,  k_r,  k_v\n"); 
scanf("%F  %F  %F",  &k_psi ,  &k_r,  &k_v) ; 

printf (" Input  k_z,  k_w,  k_theta,  and  k^qXn"); 
scanf{-%F  %F  %F  %F*,  &k_z,  &k_w,  &k_theta,  &k_q) ; 

printf { " Input  k_thruster_psi , k_thruster_r\n" ) ; 
scanf{"%F  %F",  &k_thruster_psi ,  &k_thruster_r) ; 

printf ( " Input  k_thruster„rotate\n" ) ; 
scanf  ( "%F" ,  6tk_thruster_rotate )  ; 

printf  ( •  Input  k_thruster_2 ,  k_thruster_w\n*‘ )  ; 
scanf  ■;"%F  %F",  &k_thruster_z ,  &k_thruster_wi  ; 

printf {" Input  k_propel ler_hover ,  k_surge_hover\n " ) ; 
scanf ("%F  %F",  &k_propeIler_hover,  &k_surge_hover) ; 

printf ("Input  k_thruster_hover ,  k_sway_hover\n " ) ; 
scanf f"%F  %F",  &k_thruster_hover , 

&k_sway_hcver) ; 

printf (" Input  speed-limit  from  1  to  2  feet /sec  \n"); 
scanf ( "%F" , & speed— limi t ) ; 

printf {" Input  rpm  from  +-200.0  to  +-700.0\n"); 
scanf  ( "%F" ,  &rpmi)  ; 

else  /»  use  default  initialization  values 
( 

if  (TRACE  UU  DISPLAYSCREEN} 

printf  ("[using  default  control  constant  values] \n“); 

start— dwell  =  1;  /*  delay  time  in  seconds  */ 

k— psi  =  1.00;  /*  degrees  rudder  per  degree  of  course  error  */ 

k-r  =  2.00;  /*  degrees  rudder  per  degree/sec  yaw  rate  */ 

k— V  =  0.00;  /*  needed  ??  */ 


k-w 

k-theta 


15.0 

2.0 

4.0 

1.0 


/*  degrees  planes  per  foot  of  depth  error 


*/ 


rpm 


400.0; 


k-thruster-psi  =  0.6;  /*  volts  per  1  degree  course  error  */ 

k— thruster— r  =  5.0; 

k-thruster-rotate  =  2.25;  /*  (24V) ^2=>  2  #  =  16.0  deg/sec  empirical*/ 

/*  k-thruster— rotate=  (24V  /  16  deg/sec) ^^2*/ 
k-thruster-lateral=  48 . 0;  /*  24  V  =  2  #  =  0.5  ft /sec  empirically  */ 

/*  note  voltage  follows  a  square  law  */ 
k— thruster— z  =  50.0;  /*  guesses  */ 

k— thruster-W  =  50.0; 


J^^ropeller— hover  =  200.0;  /*  200  rpm  per  one  foot  error  */ 

k-surge-hover  =  6000.0;  /*  60  rpm  per  0.01  foot/sec  surge  */ 
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this  value  is  high  to  reduce  sternway  */ 


k_thrusrer_hover  =  4.0; 

k_3way_hover  =  4C . 0 ; 


speeo^Iimit  =  2.0;  /*1.0to2.0  ft /sec  */ 

if  (TRACE  5c&  DISPLAYSCREEN) 

( 

printf  (Mk_psi  =  %5.2f,  k^r  =  %5.2f,  k^v  =  %5.2f,  k^z  =  %5.2f,  % 
k_psi,  k_r,  k_v,  k_z}; 

printf  {“k^w  =  %5.2f,  k_theta  =  %5.2f,  k_q  =  %5.2f]\n-, 
k_w,  k_theta,  k_q)  ; 

printf  (*•  [k_thruster_psi  =  %5.2f,  k_thruster_r  =  %5,2f, 
k_thruster_psi ,  k_thruster_r) ; 

printf  {“ Ik_thruster_rotate  =  %5.2f,  •, 
k_thruster_rotate) ; 

printf  Ck^thruster^z  =  %5.2f,  k_thruster_w  =  %5.2f]\n“, 
k_thruster_z ,  k_thruster_w) ; 

printf  ( "k_propener_hover  =  %5,2f,  k_surge_hover  =  %5.2f]\n*', 

k_propeI Ier_hcver ,  k_surge_hover ) ; 

printf  "k_thruster_hover  =  %5.2f,  k_sway_hover  =  %5.2f]\n“, 

k_thruster_hcver ,  k_sway_hover } ; 

rf  'ccntro2constantsfiie  =  fopen  (CONTROLCONSTANTSFILENAME ,  " w" )  )  ==  NULL) 

printf (“AUV  execution:  unable  to  open  output  file  %s  for  writing.  ", 
CONTROLCONSTANTSFILENAME) ; 

printf 

Check  ownership  penriissions  in  current  directory .  \n ; 

prinr: ( "Exit. in")  ; 
ex:  t  (  -  1  ; 


if  i'TFACE  DISFLAYSCREEN) 

printf  ( "  (controlconstantsf ile  %s  open,  pointer  =  %x]\n'', 
CONTROLCONSTANTSFILENA.ME ,  controlconstantsf  ile)  ; 

fpirnt f  f cent rol const ant sfi le , 

,  .  ”  - - — . . . .  NnXn"); 

fprintf  (controlconstantsf ile, 

"  AUV  execution  level  control  algorithm  coefficients  Xn"); 
fprintf  (controlconstantsf ile. 


fprintf  (controlconstantsf ile, 

“  k_psi  k_r  k_v  k_z  k_w  k_theta 

fprintf  (controlconstantsf ile, 

-  %5.2f  %5.2f  %5,2f  %5.2f  %5.2f  %5.2f 

k_psi,  k_r,  k_v,  k_z,  k_w,  k_thet 


k  theta. 


_  \n\n\n* ) ; 

k_q  \n\n"); 

%5.2f  \n\n\n\n", 
k«q  )  ; 


fprintf  (controlconstantsf ile, 
"  k_thruster_psi 
fprintf  (controlconstantsf ile, 
"  %5.2f 

k_thruster_psi , 

fprintf  (control constants file, 

•  k_thruster_z 

fprintf  (control const ants file, 

•  %5.2f 
k_thruster_z , 


k_thruster_r 

%5.2f 

k_thruster_r, 


k_thruster_rotate\n\n*) ; 

%5.2f  \n\n\n\n", 
k_thruster_rotate) ; 


k_thruster_w  XnXn" ) ; 

%5,2f  \n\n\n\n*, 

k_thruster_w)  ; 


fprintf  (controlconstantsfile, 

"  k_propeller_hover  k_surge_hover  NnXn"); 


40 


fprintf 

( con trcl constants fi le, 

“  %5.2f 

%5.2f 

\n\n\n\n 

h_propeller_hover , 

k_surge_hover) ; 

fprintf 

(control const ants file. 

"  k_thruster_hover 

k_s way _h over 

\n\n" ) ; 

fprintf 

( control constants fi le, 

%5.2f 

%5.2f 

\n\n\n\n 

k_thruster_hover , 

k_sway_hover ) 

f flush 

(controlconstantsf ile) ; 

f cl ose 

(controlccnstantsf ile) ; 

If  {TRACE  DISPLAYSCREEN) 

printf  ("[finish  get_cont rol^constants  ()]\n"); 

return ; 

/*  end  ger_control_constants  () 


double  dep:'th  { .) 

( 

inr  val  =  0; 

douDle  new_2  =  0.0; 
double  z_offset  =  0.0; 

If  {TRACE  &&  DISPLAYSCREEN)  printf  ("[start  depth  ()]\n-); 

:f  (LOCATIONLAB  DEADRECKON) 

/ 

z  =  2_command; 

} 

if  { N OT_ Y  ET_R  E I M  P  L EME NTED ) 

{ 

2_cffset  =  0.0; 

val  =  adt:2  (  0 , 0 )  ; 

new^z  =  0.002257  »  (val  -  2_vaI0;  +  z^offset;  /*  new_2  (ft)  */ 

else  new_2  =  2; 

if  iTR.ACE  &&  DISPL.AYSCREEN) 

printf  ("[finish  depth  (),  returns  %5-3f]\n",  new_z); 

return  fnew_z); 

}  /*  end  depth  {)  */ 

/**★**♦♦*♦********★****** ★★♦★★♦★*********^***************<^*********^^^*^*^^^^*^^ 

double  calculate_psi  () 

{ 


unsigned  short  psi_bit; 

int  psi_bi t_int ,psi_bit_old_int,delta_psi_bit; 
double  angle,  tpi;  ** 

double  pi  =  3.1415927; 

if  (TRACE  fic&  DISPLAYSCREEN)  printf  ("(start  calculate_psi  ()]\n"); 

if  (LOCATIONLAB  &&  DEADRECKON) 

{ 

psi  =  psi_command; 

} 
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f  ;NCT_YET_REIMPLEMEOTED} 


.  "*■  porr  I'i^edz  zc  be  redone;  */ 

psi_bit  =  Read_PortAB ( (struct  MFI_PIA  *)  MFI_BASE) ; 

psi_bit  &=  0x3FFF; 
psi_bit_int  =  psi_bit; 
psi_bit_old_int  =  psi_bit_old; 

delta_psi_bi t  =  psi_bit_int  -  psi_bi t_old_int ; 
psi_bit_old  =  psi_bit; 

if (abs {del ta_p£i_bit )  >  10000) 

{ 

^  wrap_count  =  wrap^count  -  del ta_psi_bit /abs (del ta_psi_bit ) ; 
tpi  =  2.0  *  pi  *  wrap_count; 
angle  =  heading-:)  -  dg_offset  +  tpi; 
angle  =  degrees  (angle); 
else  angle  -  psi; 
i:  (TRACE  DISPLAYSCREEN) 

print!  (“[finish  calculat€_psi  ()  returns  %5.3f]\n“,  angle); 
return  (normalize  (angle)); 
end  cal culate__psi  ()  »/ 


z  e  1  c^gy  r  c_^da  t  a  i  > 

int  index; 
int  save  trace; 


save_trace  =  TRACE; 

if  'TRACE  &£&  DISPbAYSCREEN)  printf  {“[start  2ero_gyro_data  OjNn” 

pitch_0  =  add  (6)  ; 

rcll_0  =  adcl(7); 

roll_rat€_0  =  adcl(9); 
pitch_rate_0  =  add  (8); 
yaw_rate_0  =  add  (10); 

2_val0  =  adc2 (0,0); 

dg_offset  =  heading (} ; 

if  (TRACE  ScSc  DISPLAYSCREEN) 

printf  ("[device  averaging  for  2  seconds, 
for  (index=0 ; index<99; ++index) 

{ 

pitch_0  +=  adcl(6); 

roll_0  +=  add(7); 

roll_rate_0  +=  adcl(9); 
pitch_rate_0  +=  add(8); 
yaw_rate_0  +=  add  (10); 
z_val0  adc2  (0,0); 

TRACE  =  FALSE; 

dg_offset  +=  headingO;  /*  this  is  verbose  if  TRACEd  */ 

trace  =  save_trace; 

tsleep  (5);  /*  256ths  of  a  second  */ 
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} 

piLch^O  =  pitch_0/100; 

roii_0  =  roll_0/100; 

roll_rate_0  =  rol l_rate_0/100 ; 
piuCh_rate_0  =  pitch_rate_0/100; 
yaw_rat:e_0  =  yaw_rate_0/ 100 ; 
z_va]0  =  z_vai0/100; 

dg_offset  =  dg_of fset/100.0; 


1 TRACE 

&&  DISPLAYSCREEN; 

1 

printf 

( "pi tch_0 

%d\n". 

pitch  0) ; 

print  f 

( " roll_0 

= 

%d\n". 

roll_0); 

■  1  i  ri  t  f 

( " roi i_rate_0 

%d\n" , 

roll_rate_0) ; 

printf 

{ "pi  tch_rate_0 

= 

%d\n" , 

pi tch_rate_0 ) ; 

print  f 

( "yaw_rate_0 

= 

%d\n" , 

yaw_ra  te_0 )  ; 

printf 

( " z_val0 

= 

%d\n" , 

z_va  10); 

printf 

( "dg_of f set 

= 

%f  \n'‘ , 

dg_of f sec ) ; 

(TRACE 

&&  DISPLAYSCREEN) 

1  printf 

■  (* [finish  zero_gyro_data 

return ; 

}  /*  end  zerc_gy ro_data  (}  */ 

/»***»»»»*»★»»♦»♦♦»♦****»»»**»★*■»*★***•**»»»♦»**»****■»»♦**»*♦»♦★»*★★***♦★★♦*★**★/ 

void  in: t iaii ze_dacs  {)  /*  Initialize  all  dac  channels  to  zero  */ 

{ 

if  (TRACE  &&  DISPLAYSCREEN)  printf  (■ [start  initialize^dacs  ()]\n*); 

If  (NCT^YET^EIMPLEMENTED) 

controi^surface  ( FRONT_RUD„TOP , 0 . 0 ) ; 
ccritrcl_surf  ace  {FRONT_RUi:'_BOT,  0.0); 
concrol.surface  (FRONT_FL_RIGHT, 0 . 0) ; 
cent rol_sur face  {FRCNT_PL„LEFT, 0 . 0 ) ; 
conr rol_surf ace  {REAR_RUD_TOF , 0 . 0 ) ; 
ccntrol_surf ace  (REAR_RUD_BOT, 0 . 0 )  ; 
ccntrol^surf ace  (REAR_PL_RIGHT, 0 . 0 ) ; 
cent rcl_surf ace  (REAR_PL_LEFT, 0 . 0 ) ; 

rr.a :  r._mot  ors_of  f  ( j  ; 


if  (TRACE  &t&  DISPLAYSCREEN)  printf  ("[finish  initialize_dacs  ()]\n"); 
return ; 

}  /•  end  ini tialize_dacs  */ 


void  ini tialize_adcs  () 

{ 

if  (TRACE  &&  DISPLAYSCREEN)  printf  ("[start  initialize.adcs  {)]\n"); 

#if  defined  (sgi) 

#else 

/*  Initialize  MFI  channels:  0  =  input  port,  1  =  output  port  ♦/ 

Init^PortA  ((struct  MFI_PIA  *)  MFI^BASE,  MFI_INPUT_PORT)  ; 

Init^PortB  ((struct  MFI.PIA  ♦)  MFI^BASE,  MFI_INPUT_PORT)  ; 

#endif 
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-f  (TRACE  DISPLAYSCREEN)  printf  ("[finish  ini tial ize_adcs  ()]\n"); 
return ; 

)  end  ini tial i 2e_adcs  */ 


void  control_surface  (surface,  angle) 


/“*■  This  function  sends  the  desired  ANGLE  to  the  specified  control  SURFACE 
The  angle  is  first  normalized  to  (-45  to  45),  then  correction  is  applied 
for  the  non-linearity  in  the  servo  control  module  */ 


int  surface; 


double  angle; 

inr  volt; 
double  a,b,c,d; 


if  (FA.LSE  &&  DISPLAYSCREEN )  printf  ("[start  control_surface  ()]\n*); 
1 f  (NCT_YET_REIMPLEMENTED) 


5  =  1.245*7^-4; 
b  =  - 2 . 9C'c  "e-l  ; 
u  =  u . ; 
d  =  500.6576; 

angue  =  angle*57 .295775;  /*  Convert  RADIANS  to  DEGREES  */ 

i:  '.(angle  <  -22.52)  It  (angle  >  22.92}) 

/*  Plane  saturated  set  to  +-  45  */ 
angle  =  22 . 92*angle/fabs (angle:; 


vc^t  =  a *pow (angle, 3 . )  +  b*pow (angle, 2 . )  +  c*angle  +  d; 
dac2b (volt , surface) ; 

) 

if  (FALSE  DISPLAYSCREEN)  printf  ("[finish  control_surf ace  ()]\n"); 
return ; 

)  /*  controi_surf ace  */ 


void  rudder  (angle) 


/*  Send  angular  deflection  (RADIANS)  to  rudders. 

Convention  (+)  angle  left  turn,  (-)  angle  right  turn  */ 

double  angle; 

( 

if  (TRACE  4c&  DISPLAYSCREEN)  printf  ("[start  rudder  ()]\n"); 

con troI_sur face  (FRONT_RUD_TOP, -angle) ; 
control_surface  (FRONT_RUD3C)T, angle)  ; 
control_surf ace  (REAR_RUD_TOP, angle) ; 
control_surface  (REAR_RUD_BOT, -angle) ; 
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:f  'TRZ^CE  DISPLAYSCREEN;-  printf  {"[finish  rudder  ()]\n"); 
rerurn; 

}  rudder  */ 

/»****it***»ir»»»********»**************»r*************»****»***»*****»*********»*/ 

veld  fianes  (angle) 

/*  Send  angular  deflection  (RADIANS)  to  bow  and  stern  planes. 

Convention  (+)  angle  dive,  {-)  angle  rise  */ 

double  angle; 

{ 

if  (TRACE  &&  DISPLAYSCREEN)  printf  {"[start  planes  ()]\n"); 

cent rol_surf ace  (FRONT_PL_RIGHT, angle) ; 
cent rol_surf ace  (FRONT_PL_LEFT, -angle) ; 
cent rol_surf ace  (REAR_PL_RIGHT, -angle) ; 
cont rol_surf ace  (REAR_PL_LEFT,  angle) ; 

if  (TRACE  DISPLAYSCREEN)  printf  ("[finish  planes  ()]\n-); 
return; 

]  / *  end  planes  ( )  * / 

/»**»»»»»»»*»»***»*★★*♦♦***»***★*»»»»»**♦*★♦♦**♦**<■****»*■★★*»»**★**♦**♦♦♦**★***/ 

vcic  n-iain^motors^of f  ()  /*  Turn  off  both  main  motors  */ 

{ 

(TRACE  DISPLAYSCREEN;  printf  ("[start  main_motors_of f  ()]\n"); 

I f  ( NOT_Y  ET_R  E 1 M  PLEMENTED ) 

dad  (612,  SUPPLY)  ; 
da  d  f  5 1 2 , R IGHT_MOTOR ; ; 
dad  .'5:2,LEFT_MOTOR)  ; 

if  TRACE  DISPLAYSCREEN)  printf  ("[  finish  main_motors_off  ()]\n"); 


return  ; 


}  /*  end  main^motors^of f  ()  */ 


/  * 


»**■***<*»*<•★■*■*■*»******★»*★*★**»♦*»****★*★■*■★★****»*★★★****★**'********  j 


void  alive  (interval,  local_start_dwell ) 


{ 


unsigned  int  interval; 

int  local_start_dwell ; 

unsigned  int  i interval , j interval ; 
double  test_delta; 

if  (TRACE  &&  DISPLAYSCREEN)  printf  ("[start  alive  ()]\n"); 


if  (NOT„YET_REIMPLEMENTED) 

{ 

local_start_dwell  =  local_start_dwel 1*100 ; 
interval  =  interval*100; 
iinterval  =  local_start_dwel 1 /interval ; 
j interval  =  0; 

test_delta  =  .4;  /*  Deflect  22.5  degrees  */ 
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while ( iinrerval  <  i interval) 

{ 

contiGl^surface  (FROKT_RUD_TOP, test_delta) ; 
tsleep ( interval ) ;  /*  256ths  of  a  second  */ 

test_delta  =  -test^delta; 
j interval  =  jinterval  +  1; 

} 

tsleep (200);  /*  256ths  of  a  second  */ 

) 

if  {TRACE  ScSc  DISPLAYSCREEN)  printf  (“[finish  alive  ()]\n-); 
return; 

} 


/Old 


\ 


record_data_on  ( ) 

if  (TRACE  &&  DISPLAYSCREEN)  printf  ("[start  record_data_on  {)]\n"); 

/*  Open  files  for  writing  */ 

if  ^  (auvdataf i le  =  fopen  (AUVDATAFILENANE, "w" ) )  ==  NULL) 

printf execution:  unable  to  open  output  file  %s  for  writing.  ", 
AITVDATAF I LENAME }  ; 

printf 

( **  Check  ownership  pentissions  in  current  directory  .  \n ; 

printf  (  "Exit .  \n‘'  )  ; 
exit  (-1 : ; 


if  (TRACE  Ui.  DISPLAYSCREEN; 

printf  ^ [ auvdataf ile  %s  open,  pointer  =  %xi\n", 

AUVDATAFILENAME,  auvda taf i le ) ; 

if  ( (auvtextfile  =  fopen  (AUVTEXTFILENAME, “w" ) )  ==  NULL) 

i 

printf ("AUV  execution:  unable  to  open  output  file  %s  for  writing  " 
AUn/TEXTF  I  LENAME  )  ; 

printf 

^ "  Check  ownership  pentissions  in  current  directory . \n ") ; 

printf ("Exit.Xn") ; 
exit  (-1 }  ; 

} 

if  (TRACE  &&  DISPLAYSCREEN) 

printf  {"[auvtextfile  %s  open,  pointer  =  %x]\n", 

AUVTEXTF I LENAME,  auvtextfile); 

fprintf  (auvtextfile,  "#  auvtextfile  %s  shows  state  ",  AUVTEXTF I LENAME ) ; 
fprintf  (auvtextfile,  "vector  variables  at  one  second  intervals . \n\n" ); ' 

if  (LOOP FOR EVER) 

fprintf  (auvtextfile,  "#  Mission  replication  #%d\n", 

replication_count ) ; 


/*  testing  code  from  wrCtl.c,  not  currently  in  use  */ 

/*  serial. d  is  a  telemetry  test  file  to  check  connectivity  */ 

/*  if  { (serialtestfile  =  fopen  ("serial. d",  “r"))  <=  0) 

printf ("AUV  execution:  record_data_on:  can't  open  test  file  serial  d\n")* 
printf { "Exit . \n" ) ;  ’  ' 

exit  (-1); 

} 

*/ 

if  (TRACE  &&  DISPLAYSCREEN)  printf  ("[finish  record_data_on  {)]\n"); 
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return ; 


if  (TRACE  DISPLAYSCREEN)  printf  {*  {start  record_data_of f  ()3\n*); 

if  {TRACE  &&  DISPLAYSCREEN) 

printf  ("[flushing  and  closing  auvdatafile  %s  %x]\n'', 

AUVDATA FILENAME,  auvdatafile); 

f flush  (stdout) ; 

} 

if  (auvdatafile  !=  NULL) 

{ 

if  (TRACE  &&  DISPLAYSCREEN)  printf  ("[auvdatafile  f lushed] \n- ) ; 
f flush  (Stdout) ; 
f flush  (auvdatafile); 
fclose  (auvdatafile) ; 

if  (TRACE  ScU  DISPLAYSCREEN)  printf  ("[auvdatafile  closed]  \n*); 
f flush  (stdout); 

}_ 

else  if  (TRACE  &&  DISPLAYSCREEN)  printf  ("[auvdatafile  was  not  open!!]\n"); 

If  (TRACE  &&  DISPLAYSCREEN) 

print!  (“[flushing  and  closing  auvtextfile  %s  %x]\n", 

AUVTEXTFILENAME,  auvtextfile); 

ffjush  (Stdout); 

} 

if  (auvtextfile  •=  I'TULL) 

{ 

if  CTRACE  DISPLAYSCREEN)  printf  ("[auvtextfile  f  lushed]  \n" )  ; 
f flush  (stdout;; 
f flush  (auvtextfile) ; 
fclcse  (auvtextfile) ; 

if  (TRACE  LSt  DISPLAYSCREEN )  printf  ("[auvtextfile  closed]  \n"); 
f flush  (Stdout ; ; 

} 

ej.se  if  (TRACE  DISPLAYSCREEN)  printf  ("[auvtextfile  was  not  open!l]\n"); 

/'  fclose  (serialtestfile) ;  /*  serial  port  test  file  */ 

if  (TRACE  &&  DISPLAYSCREEN) 
i 

printf  ("[finish  record_data_of f  ()]\n"); 
f flush  (stdout); 

} 

return ; 


/***************'k***ik********±*if**ii-k*ic**icic*^±icitic**i:i(it^icitit*ic^1titicicicicii***it***it1i1tiiic^ 

void  record_data  () 

{ 

/♦  temporary  hold  variables  */ 

double  AUV_tiiiie_teinp, 

AUV_x_teinp, 

AUV_phi_teinp , 

AUV_u_teinp, 

AUV_p_temp, 

AUV_x_dot_teinp , 


AUV_y_teinp , 
AUV_thet  a_teinp , 
AUV_v_teinp , 
AUV_q_teinp , 
AUV_^_dot_teinp , 


AUV_2_teinp, 
AUV_psi_teinp, 
AUV_w_teinp, 
AUV^r^temp, 
AUV_2_dot_temp , 
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Air';_phi_dot_temp,  Air\/_theta_dot_teinp,  AUV_psi_dot_t:emp, 

AUV_del  Ca_rudder_temp ,  AUV_delta_planes_teinp, 

ALV^p  c  r  t  _rpm_t  emp ,  AUV_s  t  bd_r  pra_t  emp , 

A  LA^  _b  o  w_v  e  r  t  i  c  a  1  _  t  eirp ,  AUV_s  t  e  r  n_v  ertical_t  emp , 

AUV_bow_lateral__teTnp,  AUV^stern^lateral^teinp/ 

AUV__ST1 00  0_range_teinp ,  AUV_ST7  2  5_range_temp , 
AUV_ST1000_bGaring_temp ,  AUV_ST72 B^bearing^temp , 

AUV_S  TlOOO^strengi:  h_t  emp ,  AUV_ST7  2  5_s  t  r  e  ng  t  h_t  emp  ; 

if  (TRACE  &&  DISPLAYSCREEN)  printf  {-[start  record^data  ()]\n-); 

S' teiT^^t line  =  tiine  (NULL); 

systeiT;_tmp  =  localtime  {&systerri_time)  ; 

If  [TRACE  LISPLAYSCREEN) 

printf  {"[OS-9  systerri  time  is  %02d :  %02d:  %02d,  replication  %d]\n-/ 

sy s tem_tmp- >tm_hour ,  sy scem_tmp->tm_min,  system_tmp->tm_sec , 
repl: cationic ount ) ; 

255;  i^+) 

'\0';  /*  zero  buffer  */ 


size  =  sprintf  (buffer, 

ite  %5.3f  %5.3f  %B.3f  %B.3f  %B.3f  %B,3f  %5.3f  %B.3f  %B.3f  %5.3f  %B.3f 
:f  %5.3f  %5.3f  %5.3f  %5.3f  %5.3f  %B.3f  %5.3f  %5.3f  %B.3f  %5.3f  %5.3f  %B.3f 
:f  %B.3f  %5.3f  %5.3f  %B.3f  %5.3f  %5.3f  Xn", 
t , x,y , 2 , 

norTriali2e2  (thetaj, 
noriTialize  (psi  ]  , 


normalize/  (p) , 

norTnaIi2e2  (q)  , 

normal i ze2  ( r ; , 

x_dot,  y_dot,  2_dot, 

ncrTTializeC  (phi_doc  ), 

norTnaiize2  (theta_dot}, 

ncrmali2e2  (psi_dot  ), 

normalize/  ( delta_rudder ) , 

ncrmalizeC  (delta_planes ) , 

pcrt_rprri,  stbd_rpm, 

AU\'_bcv._vertical ,  AUX'^stern^vertical 

AEP7_bow_Iateral ,  AUV_stern_lateral , 

Airc_STl  C  0  0_bGa  r  i  ng ,  AUV_ST1 0  0  0_range , 

AErv_ST7  2  5_bear  ing ,  AUV_ST7  2  5_r  ange , 


AW_ST1 0  0  0_s  t  rength , 
AUV_£T725_strength) ; 


(buffer_size  >  2B4) 


/*  sprintf  buffer  overflow  condition 


:t  '.DISPLAYSCREEN) 

printf  (“Buffer  overflow,  buffer_si2e  =  %d,  reduced  to  254  l!!!!!\n-y 

buf fer_size) ; 

buffer^size  =  254; 

(TRACE  DISPLAYSCREEN)  printf  ( -  [buf  f er_size  is  %d]\n-,  buf  fer_size)  ; 


/*  other  state  variables  &  timing  constraints 


send_data_on_virtual_world_socket  ();  /*  there  it  goes  */ 

if  (TRUE  &&  DISPLAYSCREEN)  telemetry  report  to  screen  */ 

printf  (“Xnsending  to  virtual  world:-); 


48 


printf  ("\n%s'‘,  buffer); 

) 

el.-e  if  (DISPbAYSCREEN)  /*  partial  telemetry  report  */ 

{ 

printf  ("Xnsent  telemetry'  to  virtual  world  %5,3f  ",  t); 


get:_stream_f rom_virtual_wcrld_socket  ();  /*  here  it  comes  *l 

/*  update  and  output  AUV  state  variables  */ 

/*  note  that  if  dead  reckoning  is  used,  values  will  not  change  */ 

variables_parsed  =  sscanf (buf f er_received, 

"%s  %lf  %lf  %if  %lf  %lf  %lf  %lf  %lf  %lf  %lf  %lf  %lf  %lf  %lf  %lf  %lf  %lf 
%lf  %lf  %lf  %lf  %lf  %lf  %lf  %lf  %lf  %lf  %lf  %lf  %lf  %lf  %lf  \n",*/ 

"%s  %F  %F  %F  %F  %F  %F  %F  %F  %F  %F  %F  %F  %F  %F  %F  %F  %F  %F  %F  %F  %F  %F  %F 
%F  %F  %F  %F  %F  %F  %F  %F  %F  \n", 

keyword,  tAUV^t ime_temp , 

itAUV_x_t  emp ,  fiLAUV_y_temp ,  fiiAUV_2_temp , 

£tAW_phi_temp ,  fitAUV_theta_temp,  &AUV_psi_temp , 

&:AUV_u_t  emp ,  &AUV_v_t  emp ,  &  AUV_w_t  emp , 

&  A  W_p_t  emp ,  StAUV_q_t  emp ,  &  AUV_r_t  emp , 

&Airv"_x_dot_t  emp ,  &AUV  _y_dot_temp ,  &AUV_2_do  t_temp , 

&:ALrv_phi_dot_temp,  &AUV_theta_dot_temp,  fitAUV_psi_dot_temp, 

itAW^del  ta_rudder_temp ,  ScAlTV_del  ta_j3lanes_temp, 

&AU\'_por  t„rpm_tem.p ,  £cAUV_stbd_rpm_temp , 

A  'Jv^^bow^v  ertical_t  emp ,  £tAUV_s  t  e  r  n_ve  rtical_t  emp , 

&AUV^_bow_l  ateral_temp,  ficAUV_stern_lateral_temp, 

4tAUY^_STi  000_bearing_temp ,  SlAUV^STI  000_range_temp, 

ficAUV_STl  0  0  0_s  t  reng  t  h_t  emp , 

&cAU'v_ST7  25_bearing_temp,  &LAUV_ST725_range_temp, 

fitAUV_ST725_strength_temp)  ; 


' van abies_parsed  ==34]  /*  transfer  was  OK  so  keep  new  values  */ 


X  = 

y 

z  = 

pni 

tiieta  = 

psi 

V  = 

w  = 

p 

q  = 

r  = 

x_dot  = 

y_dot  = 

z_dot  = 

phi_dot  = 

theta_dot  = 

psi_dot  = 

delta_rudder  = 

delta_planes  = 

port_rpm  = 

stbd_rpm  = 

AUV_bow_vertical  = 

AUV_stern_vertical  = 

AUV_bow_lateral  = 

AUV_stern_lateral  = 

AUV_ST1000_bearing  =nonnal 

AUV_ST1000_range  = 

AUV_ST1000_strength  = 

AUV_ST7  2  5_bea  r i ng  =norma 1 

AUV_ST7 2 Strange 


( AtPy_time_temp)  ; 
(AU\^_x_temp)  ; 

(AU\'_y_temp)  ; 

{AUV_2_temp) ; 

(AUV_phi_temp) ; 

( AUV_the  t  a_temp ) ; 
{AUV_psi_temp) ; 

{AUV_u_temp) ; 

(AUV_v_temp) ; 

(AUV_w_temp} ; 

(AUV_p_temp)  ; 

(AUV_q_temp ) ; 

(AlJV_r_temp )  ; 
{AUV_x_dot_temp)  ; 

{ AUV_y_do  t_t  emp )  ; 
(AUV_2_dot_temp)  ; 
{AUV_phi_dot_temp)  ; 
{AUV_theta_dot_temp)  ; 
(AUV_psi_dot_temp)  ; 
(AUV_delta_rudder_temp) ; 

( AUV_de  1 1 a_p lane s_t emp ) ; 
(AUV_port_rpm_temp)  ; 
(AUV_stbd_rpm_temp)  ; 
{AUV_bow_vertical_temp) ; 
(AUV_stern_vertical_temp)  ; 
(AUV_bow_lateral_temp)  ; 
(AUV_stern_lateral_temp)  ; 
Lze(AUV_ST1000_bearing_temp) ; 
(AUV_ST1000_range_temp)  ; 
(AUV_ST1000_strength_temp)  ; 
Lze (AUV_ST725_bearing_temp} ; 
(AUV_ST725_range_temp) ; 
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^U'v"_ST7  25_strength  = 


{ AUV_ST72  5_strength_terop)  ; 


else  if  { (variables^parsed  !=  34)  &&  ( variables_parsed  !=  -1)) 
if  (DISPLAYSCREEN) 

printf  ("\nGarble  problem  in  buf f er^received  !!!  variables  parsed  =  %d\n%s\n" 
variables_parsed,  buffer_received} ; 
answer  =  getchar  ();  /*  pause  */ 

TRACE  =  TRUE; 

} 

if  •; bOCATIONLAB  ==  0}  speed  =  u;  /*  virtual  world  speed,  not  flow  sensor  */ 
If  (TRACE  &&  DISPLAYSCREEN) 

printf  (“Nufrom  virtual  world  buf f er_received : \n%s " ,  buf fer_received) ; 

if  (TRUE  &&  DISPLAYSCREEN) 

printf  ("\nfrom  virtual  world  state  variables:"); 
printf  ("Xn  %s  t=%5,3f  x=%5.3f  y=%5.3f  2=%5,3f  ", 
keyword,  t, 

y.  z); 

printf  ("phi=%5.3f  theta=%5.3f  psi=%5.3f  ", 

'  theta,  psi )  ; 

printf  !"u=%5.3f  v=%B.3f  w=%5.3f  p=%5.3f  q=%5.3f  r=%5.3f  ", 

''-i  /  V  ,  W  , 

.  P'  Q/  r); 

pnntr  ( ’’x_dot  =  %5 . 3f  y_dot  =  %5.3f  2_dot  =  %5.3f  ", 

x_dot,  y_dot,  z_dot}; 

print  f  ( "phi_dot  =  %5 . 3f  theta_dot=:%5 . 3f  psi_dot  =  %5 . 3f  ", 
phi_dot,  theta__dot,  psi_dot); 

prlntf_  {"delta_rudder=%5.3f  del ta_planes=%5 . 3f  ", 
del t a_rudder ,  del ta_planes ) ; 

printf  ( "pcrt_rpm=%5 . 3f  stbd_rpm=%5 . 3f  ", 
port_rpm,  stbd_rpm) ; 

printf  ( "bow_vertical=%5 . 3f  stern_vertical=%5 . 3f  " , 

AUV_bow_vertical ,  AUV_stern_vertical ) ; 
printf  ( "bow_lateral=%5 . 3f  stern_lateral=%5 . 3f  ", 

AUV_bow_lateral ,  AUV_stern_lateral ) ; 
printf  f-STlOOO  b/r/s  %5.3f  %5.3f  %5.3f,  ST725  b/r/s  %5.3f  %5.3f  %5.3f", 
ALT v’_STl  0  0  0_bea  r  i  ng ,  AUV_ST1 0  0  0_ra  nge ,  AUV_ST1 0  0  0_s  t  reng  t  h , 
AU\^_ST725_bearing,  ALf\^_ST725_range,  AUV_ST725_strenath)  ; 
printf  [current  time  %d  %d  %d]  \n", 

system_tmp->tm._hour,  system_tmp'>>tm_min,  system_tmp->tm_sec)  ; 


/*  keep  all  telemetry  variables  in  degrees  */ 
phi  =  normalize2  (phi  ) ; 

theta  =  normalizes  (theta); 

ps-  =  normalize  (psi  ) ; 

phi_dct  =  normalizes  (phi_dot); 

theta_dot  =  normalizes  (theta_dot); 

psi_dot  =  normalizes  (psi_dot); 

P  =  normalizes  (p); 

q  =  normalizes  (q) ; 

r  =  normalizes  (r); 

delta_rudder  =  normalizes  (delta_rudder) ; 
delta_planes  =  normalizes  (delta^planes) ; 


if  (TRACE  &&  LOCATIONLAB  &&  DISPLAYSCREEN) 


printf  ("- 


if  (auvdatafile  !=  NULL) 
{ 

if  (buffer_size  ==  0) 


I*  output  data  to  telemetry  file  */ 
/*  note  that  unmodified  stream  is  saved  */ 
/*  nothing  was  received,  send  auv^^state  */ 
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fprintf  (auvdataf ile,  "%s'',  buffer); 

/*  feedback  was  received,  send  uvw__state  *  / 
fprintf  {auvdatafile,  -%s-,  buffer_received) ; 

if  (TFACE  &&  DISPLAYSCREEN) 

printf (“ [printed  to  %s  telemetry  file]\n",  AUVDATAFILENAME) ; 

f  ■  .  ■ 

/*  only  send/print  out  every  10th  telemetry  entry  to  tactical  level  */ 

due  to  serial  port  bandv;idth  limitations  :-(  */ 


/*  rede  this  when  clock  logic  is  redone  **************★***★★***♦*****★★**★**★**/ 

if  {  ;intM  (t-^0.05)  *10.0}  ==  (int)  (t+0.05)  *10) 

\ 

if  (TACTICAL  Sci.  TRACE  &&  DISPLAYSCREEN) 

printf  (-[sending  data  to  tactical  level] \n*); 

*af  defined  (sgi) 

^  fe*  se 

/*  writeln  (serialpath,  buffer,  255);  ««««<«  */ 

if  (TACTICAL  ==  TRUE)  write  (serialpath,  buffer,  255); 

tendif 

if  (TACTICAL  &&  TRACE  &&  DISPLAYSCREEN) 

printf  (-[write  buffer  to  tactical  level  serialpath  OK]\n"); 

:f  'auvcextfile  !=  NULL)  /*  output  data  to  .auv  text  file  */ 

if  (TRACE  kh  DISPLAYSCREEN; 

printf  (-[sending  data  to  .auv  text  file]\n-); 

fprintf  (auvtextf ile,  *%s",  buffer); 

if  (buffer_size  !=  0)  /*  feedback  was  received,  also  send  uvw_state  */ 
fprintf  { auvtext f i le,  -%s-,  buffer^received) ; 

If  (TRACE  DISPLAYSCREEN) 

printf  (- [fprintf  to  .auv  text  file  OK]\n-); 

) 


telemet ry_records_saved  -f-n; 


If  ( ( (buf fer_index  +1)  %  FILEBUFFERSIZE)  ==  0)  buffer_index  =  0; 

buffer_index  ++; 

/*  need  to  copy  buffer  to  buffer_array  if  caching  telemetry  «««««« 
if  {TRACE  tcu  DISPLAYSCREEN)  printf  ( -  [buf  f er_index  =  %d]\n“,  buf  fer_index)  ; 


_ _ 

/*  test  code  to  send  data  from  file  serial. d  from  wr2tl,c 
/*  read  characters  from  file,  echo  characters  to  screen, 

/*  send  characters  to  serialpath  /tl  device 
/*  while  ((c[0]  =  getc (serialtestf ile) )  !=  EOF) 

putc  (c [0 ] , stdout ) ; 
writeln  (serialpath,  c,  1); 

) 

*/ 

- - 

if  (TRACE  &&  DISPLAYSCREEN)  printf  ("[finish  record_data  ()]\n"); 


*/ 

*/ 

*/ 


*/ 


return; 

) 
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douD^e  roIl_angle  ()  /*  Return  roll  angle  in  RADIANS 

int  val ; 

double  angle  =  phi;  /*  previous  {or  virtual  world)  value  */ 

if  (TRACE  &&  DISPLAYSCREEN)  printf  ("[start  roIl_angle  ()]\n"); 

if  (NOT^YET^REIMPLEMENTED) 

{ 

val  =  add  (ROLL_ANGLE_CH)  ; 

anoif  "  T  val) /5. 7572) /57. 295779;  convert  to  radians  V 

^  angle  =  (-.1737*val  +  .1737*roll_0) /57 .295779; 

angle  =  norinalize2  (angle); 
if  (TRACE  DISPEAYSCREEN) 

printf  (-[finish  roll_angle  ()  returns  %5.3f]\n*,  angle); 
return  (angle) ; 


f*»******1H 


doubirr  L:tch_angle  ()  /*  Return  pitch  angle  in  RADIANS 

inr  val; 

acuble  angle  =  theta;  /-  previous  (or  virtual  world)  value  */ 

If  .'TRACE  kk  DISPLAYSCREEN)  printf  ("[start  pitch_angle  ()]\n“); 
i  f  'NOT__YET_P.EIMPLEMENTED) 

val  =  adci (PITCK_ANGLE_CH) ; 

angle  =  ({520.153  -  val ) /8 .340) /57 .295779;  convert  to  radians  •/ 
angle  -  -  (  (- .  1199’’val  4  . 1 199»pitch_0)  757.295779); 


angle  =  normali2e2  (angle); 
if  (T.RACE  &&  DISPLAYSCREEN) 

printf  (“ [finish  pi tch_angle  ()  returns  %5.3f]\n-,  angle); 
let urn  (angle) ; 


double  heading  () 


''  heading  angle  with  respect  to  local  magnetic  north  in  radians 

fiom  directional  gyro  */ 

uni.oigned  short  dg_bi  t ; 
double  angle; 

if  (TRACE  DISPLAYSCREEN)  printf  (-(start  heading  ()]\n-); 
if  (LOCATIONLAB  &&  (DEADRECKON  ==  FALSE)) 

angle  =  psi; 

) 

else  if  (LOCATIONLAB  (DEADRECKON  ==  TRUE)) 


52 


angle  =  psi^coininand; 


{ 

I 

else 

i 

if  (NOT_YET_REIMPLEMENTED} 

{ 

dg_bit  =  Read_PortAB(MFI_BASE) ; 
dg_bit  6f=  0x3 FEE; 

} 

angle  =  (3.8350e-4)  *  dg_bit; 

} 

angle  =  normalize  (angle); 
if  (TRACE  Uf.  DISPLAYSCREEN) 

printf  ("[finish  heading  {)  returns  %5.3f]\n",  angle); 
return  (angle) ; 

) 

/lttir-K**’k**ie*i[ieirifti*ir***ic*tr*****ie*ic*-ic-*'kie*****************************************/ 

double  roll_rate_gyro  {)  /*  Return  roll  rate  in  RADIANS/SEC  */ 

\ 

int  val ; 

double  rate  =  p;  /*  previous  (or  virtual  world)  value  */ 
if  fTRACE  &&  DISPLAYSCREEN)  printf  ("[start  roll_rate_gyro  ()]\n"); 
defined  'sgij 

val  =  add  (ROLL_RATE_CH)  ; 

ra-e  =  (roll_rate_0/3 . 2113  -  . 31062*val ) /57 . 295779 ; 

#endi  f 

rate  =  normali2e2  (rate) ; 
if  (TRACE  DISPLAYSCREEN) 

printf  {"[finish  rcli_rate_gyro  ()  returns  %5.3f]\n",  rate) ; 
reiurn  (race); 


/^y,^t-K*-,c’^**-f**-»W>rir**i,*yc^****-k******1f****i>*^*it*********-k-k*9************-K*****it**/ 

douhle  pi tch_rate_gyro  ()  /*  Return  pitch  rate  in  RADIANS/SEC  */ 

{ 

int  val  =  0; 

double  rate  =  q;  /*  previous  (or  virtual  world)  value  */ 

if  (TRACE  ScSc  DISPLAYSCREEN)  printf  ("(start  pitch_rate_gyro  {)]\n"); 

#if  defined  (sgi) 

#else 

val  =  add  (PITCH_RATE_CH)  ; 

rate  =  (pitch_rate_0/13 . 69399  -  .0730001*val) /57 .295779; 

♦endif 

rate  =  nonnalize2  (rate) ; 
if  (TRACE  DISPLAYSCREEN) 

printf  (• (finish  pitch_rate_gyro  ()  returns  %5.3f]\n",  rate); 
return  (rate); 

} 
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r-*-**-*-**-**--*******. 


double  yaw_race_gyro  ()  /*  Return  yaw  rate  in  RADIANS/SEC 

Int  val  =0; 

double  rate  =  r;  /*  previous  (or  virtual  world)  value  */ 

if  (TRACE  &&  DISPLAYSCREEN)  printf  ("[start  yaw_rate_avro  ()]\n“); 

#lf  defined  (sgi) 

#else 

val  =  add  (YAW_RATE_CH}  ; 

rate  =  {yaw_rate_0/13 . 653216  -  . 0732362*val ) /57 . 295779 ; 

#endi f 

rate  =  normali2e2  (rate) ; 
if  (TRACE  &&  DISPLAYSCREEN) 

printf  (- [finish  yaw_rate_gyro  ()  returns  %5.3f]\n-,  rate) ; 
return  (ratei; 


r.uD.e  stbd_iT;Ctor_rpit  ()  j*  Reads  rpin  from  RIGHT_MOTOR 

int  pulse; 
double  local_stbd_rpm; 

i:  (TRACE  ick  DISPLAYSCREEN)  printf  ("[start  stbd_motor_rpm  ()]\n"); 

pulse  =  add  (RIGHT_MOTOR_RPM)  ; 

Iocal_stbd_rpm  =  1.244*pulse  -  8.4792; 

if  (TRACE  kk  DISPLAYSCREEN) 

printf  ("[finish  stbd__motor_rpm  ()  returns  %5.3f]\n-,  stbd_rpm)  ; 
return  t local_stbd_rpm; ; 


double  pcrt_motor_rpir:  {j  /*  Reads  rpm  from 


rpm  from  LEFT_MOTOR 


int  pulse; 
double  local_port_rpm; 


If  (TRACE  DISPLAYSCREEN)  printf  (> [start  port_inotor_rpm  ()]\n-); 

pulse  =  add (LEFT_MOTOR_RPM) ; 
iocal_port_rpiT!  =  1.244*pulse  -  8.4792; 

if  (TRACE  &£<  DISPLAYSCREEN) 

printf  (“ [finish  port_motor_rpm  ()  returns  %5.3f]\n*,  local_port_rpjn) ; 
return  (local_port_rpin) ; 


double  get_speed  ()  /*  Filter  the  speed  signal 

int  index; 

double  avg_speed  =  0.0; 


if  (TRACE  &&  DISPLAYSCREEN) 

printf  (‘[start  get_speed  (),  LOCATIONLAB=%d] \n' ,  LOCATIONLAB) ; 
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if  {LOCATIONLAB] 
i 

if  (TRACE  &&  DISPLAYSCREEN} 

printf  (-[finish  get_speed  ()  returns  • ) ; 
avg_speed  =  (speed_per_rpin  *  (port_rpit  +  stbd^rpiri)  /  2.0); 
if  {TRACE  &&  DISPLAYSCREEN) 

printf  { •'%5 .3f  ]  \n" ,  avg_speed); 

return  (avg_speed) ; 

] 

else 

t 

speed_array [pointer]  =  adc2(l,0); 
for  (index=0 ; index<ll ; ++index) 

( 

avg_speed  +=  speed_array [ index] ; 

} 

avg_speed  =  avg_speed/ll ; 
avg_speed  =  avg_speed  *  0.003635; 
if  (avg_speed  <  0.78} 

( 

if  (t  <  10.0) 

avg^speed  =  speed  +  0 . 000045*  (rpit-lBO  )  ; 
if  (avg_speed  >  (rpin*0 . 002 )  ]  avg^speed  =  rpin*0.002; 

} 

else 

{ 

avg_speed  =  1.0; 

) 

i f ( t  <  2.0)  avg_speed  =  0.0; 

pointer  =  pointer  1; 
if  (pointer  >  10)  pointer  =  0; 


if  (TRACE  &&  DISPLAYSCREEN) 

printf  (-[finish  get_speed  {)  returns  %5.3f]\n-,  avg^speed) ; 
return  (avg^speed; ; 


void  get_init_avg  () 

( 

int  index,  rng_su:Ti; 

if  .TRACE  US.  DISPLAYSCREEN)  printf  {-[start  get_init_avg  ()]\n-); 

rng_sum  =  0 ; 

range_index  =  0; 

for (index  =  0;  index  <  AVG_PTS;  ++index) 

^  via01ORB_IRB]  =  (SONAR_SWl  &  SONAR_SW3)  I  SONAR_TRIG2; 
viaO[ORB_IRB)  =  SONAR_SWl  &  SONAR_SW3; 
tsleep (5) ; 
range  =  adc2 (3,0) ; 

rng_suiTi  +=  range; 
range_array [index]  =  range; 

++range_index; 


avg_rng  =  ( rng^suin/AVG^PTS)  *  1.0; 

if  (TRACE  &&  DISPLAYSCREEN)  printf  ( M finish  get_init_avg  ()]\n*); 
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return; 


void  get_avg_rng  (} 

{ 

inc  index,  UPDATE_AVG,  int_rng_suin; 

if  (TRACE  &&  DISPLAYSCREEN)  printf  C (start  get_avg_rng 

UPDATE_AVG  =  0 ; 
int_rng_suiD  =  0; 

if  {(( float ) range  >  avg_rng  )  M 

(fabs ( (float ) range  *  avg_rng)  <=  MAX_RNG_DIFF)  II 
(bad_rng  >=  MAX_BAD_PTS) ) 

{ 

range_array [range_index]  =  range; 

-^'^range_index; 

UPDATE_AVG  =  1; 
if(bad_rng  >  MAX_BAD_PTS) 

+  -rbad_updates  ; 

i f (bad_updates  >=  MIN_NC_PTS} 

{ 

bad_rng  =  0 ; 


^+bad_rng; 


for (index  =  range_index  -  AVG_PTS;  index  <=  range_index;  ++index) 
int_rrjg_suir.  +=  range_array  [index]; 


avg_rng  =  int_rng_sujt/AVG_PTS  *  1.0; 

i 

:f  (TRACE  i£<  DISPLAYSCREEN)  printf  (“[finish 


ish  get_avg_rng  ( ) ] \n“ ) ; 


Hardware  control  changes  *FOLLOWING*  hardware  upgrade  1993: 


Telemetry  to  tactical  level: 
Orders  from  tactical  level: 


Sonar : 


serial  port  /T1  via  driver  /TT 
parallel  port  /P  via  MFI  register  A 
interface  card  device  driver  /T3 


/************************^****************^**^***^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ 

void  open_device_paths  () 

{ 

if  (TRACE  &&  DISPLAYSCREEN)  printf  ("[start  open_device_paths  ()]\n"); 

#if  defined  (sgi) 
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eichti  /tl  serial  port  #1  or  /tt  (high  baud  rate  driver  for  /tl)  j 

serialpath  =  open  C/tl",  S_IREAD  +  S_IWRITE)  ;  /*  get  path  nuiT±)er  ♦/ 

/*  /tt  is  device  for  high  baud  rate  /tl  serial  port  */ 

if  {serialpath  <=  0) 

{ 

printf  ( "open^device^paths  ():  unable  to  open  serialpath  /tl.  ■); 
printf  ( "Exit . Xn” ) ; 
exi t  ( -1 ) ; 

) 

if  {TRACE  &&  DISPLAYSCREEN) 

printf  ("[serialpath  /tl  (normal  baud  rate)  open,  path  number  =  %d]\n", 

serialpath) ; 

if  (SONARINSTALLED  ==  TRUE) 

{ 

sonarpath  =  open  {"/ta",  S_IREAI)  +  S_IWRITE) ;  /*  get  path  number  */ 

/*  /t3  is  device  for  sonar  interface  card  */ 

if  (sonarpath  <=  0) 

( 

printf  { "open^device^paths  ():  unable  to  open  sonarpath  /t3 .  ■); 

printf  { "Exit . \n “ ) ; 
exit  ( -1 ) ; 

} 

if  (TRACE  &&  DISPLAYSCREEN) 

printf  ("[sonarpath  /t3  open,  path  number  =  %d]\n'‘,  sonarpath); 

tty_mode  i sonarpath , 1 } ;  /*  initialize  sonar  values  */ 

else  if  (TRACE  DISPLAYSCREEN) 

printf  (“[sonarpath  /t3  ignored,  SONARINSTALLED  ==  FALSE] Xn*); 

/*  other  paths:  effectors,  d€pth_sonar,  etc.  ***********»**♦*******♦****★/ 

^endi f 

It  (TRACE  hk  DISPLAYSCREEN)  printf  ("[finish  open_device_paths  ()]\n"); 
return ; 

} 

/»»»»*»•«**«••»**«★*♦*»•*»*★*■**»★**★»************»*★★★**★**★***♦**★♦*★♦★**★*★*♦*★★*/ 

void  cl ose_device_paths  () 

if  fTRACE  kk  DISPLAYSCREEN)  printf  {"[start  close_device_paths  ()]\n"); 

if  (serialpath  >  0)  close  (serialpath);  /*  test  for  open  before  closing  */ 

else  if  (TRACE  kk  DISPLAYSCREEN)  printf  ("[serialpath  was  not  open!]\n"); 

if  (SONARINSTALLED) 

{ 

if  (sonarpath  >  0)  close  (sonarpath); 

else  if  (TRACE  kk  DISPLAYSCREEN)  printf (•[ sonarpath  was  not  open!]\n"); 

) 

/*  other  paths:  effectors,  depth_sonar,  etc.  <<<<<<<<<<<<<<<<<<<<<<<<<<  */ 
if  (TRACE  kk  DISPLAYSCREEN)  printf  ("[finish  close_device_paths  ()]\n"); 
return; 

) 

void  read_parallel_port  ()  /*  loop  and  display  8  bit  data  from  port  A  */ 

{ 
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static  char  nGXt_char,  last_char; 
static  char  current_coinmand  [256]; 
static  int  index; 
unsigned  char  temp; 


if  (TRACE  &&  DISPLAYSCREEN) 

printf  ("[start  read_parallel_port  {)/*■); 

if  (PARALLELPORTTRACE)  printf  { " PARALLELPORTTRACE  is  ON] \n  ); 
else  printf  ("PARALLELPORTTRACE  is  OFF]\n  ); 


/*  see  initialize_adcs  ()  for  Init_PortA  &  B  code 


V 


#if  defined  (sgi) 

"/*  Read  PortA  parallel  port  character  by  character  for  tactical  orders  */ 
r  reference:  Walt  Landaker's  mfi_a3.c  in  directory  /hO/AUV  and  I 

/*  paae  3-12  of  Motorola  6800  Series  Manual  for  6821  PIA 

/*  Programmable  Interface  Adapter. 

/"  Warning!  You  may  have  to  reset  both  computers  to  get  the  parallel  V 

port  to  read  ^  write  properly.  Additionally, 

on  the  386  you  can  run  PORTFIX  to  reset  parallel  port  LPTl :  */ 

temr.  =  Read.PortA  {(struct  MFI^PIA  *)  MFI_BASE) ;  /"  should  clear  busy!  */ 


0; 


read  pert  status  (note  sta  not  stb) 

PcrtAFlag  =  ck_sta  ((struct  MFT_PIA  *)  MFI_BASE) ; 


if  f PARALLELPORTTRACE  &&  DISPLAYSCREEN) 

printf  ("\n  [time  %5.2f  read_parallel jort  ()  resumed]",  t)  ; 

while  (PortAFlag  &&  0x80)  see  loop  break  for  alternate  exit  */ 

/*  Note  that  ck_stb  is  used  in  mfi„a3  but  ck_sta  makes  more  sense  */ 

PcrtAFlaa  =  ck^sta  ((struct  MFI^PIA  *)  MFI^BASE) ;  /*  read  port  status  */ 
last  cnar  =  next^char;  /*  read  char  and  reset  busy  * ! 

nextichar  =  Read_PortA  ■' (struct  MFI_PIA  MFI_BASE}  ; 


if  ((PcrtAFlag  ==  0x24)  &&  {last_char  ==  next_charj )  break; 

/*  if  nextichar  changed  then  flag  may  be  messed  up,  read  anyway  */ 

/*  check  for  ptr  strobe  */ 

/*  break  =>  no  character  waiting  */ 

/*  control  passes  outside  while  loop  */ 

else  if  (next_char  ==  13)  /*  CR  indicates  end  of  line  *J 

current_command  [index]  =  13;  CR  /n 

current^command  [index-1 ]  =  10;  /*  LF  extra,  not  needed  */ 

current^command  [ index -^2]  =0;  /*  end  of  string  delimiter  */ 

index  =  0; 


if  (auvtextf ile! =NULL) 

^  fprintf  (auvtextfile,  •%s",  current_command) ; 
ff lush  (auvtextfile) ; 

} 

if  (DISPLAYSCREEN) 

^  printf  (•\n\n»>  time  %5.2f  tactical  message  «<\n“,  t); 
printf  (“%s",  current_command) ; 
printf  (*\n"); 

/★★**★♦**  insert  command  processing  logic  here  ««««««< 


V 
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else  if  (next__char  !=  10) 


/*  LF  ignored,  others  appended  */ 


current_coiTunand  (index]  =  next_char; 
index  ++; 

if  {PARALLELPORTTRACE  DISPLAYSCREEN ) 

{ 

/*  print  character  to  screen  */ 

printf  CNn  %c  =  %2x,  PortAFlag  after  reading  =  %4x] 
next_char,  next_char,  PortAFlag); 

} 

} 

)  /*  end  while  loop  to  read  characters  from  port 

if  (PARALLELPORTTRACE  DISPLAYSCREEN) 

printf  ("  [%c  =  %2x,  PortAFlag  =  %2x,  exiting  read_jDarallel_port  ()]*, 

next_char,  next_char,  PortAFlag); 

} 

#endi f 

if  (TRACE  &&  DISPLAYSCREEN)  printf  (M  finish  read^parallel  jort  {)]\n*); 


)  1'^  end  read_para Heliport  */ 

y^»»»»»»»»^*»»»*'**************************-**-*»*r**r*****<ir*************»****»-******/ 

’  Card  arrangemeni:  in  ALV  *PRIOR*  to  hardware  upgrade  1993: 


Main  motors 


1  f 

1  1 

1  1 
!  1 

MFI 

Reg  A  and  B 

1  Direct 

1  Gyro 

1 

I 

adc-2 
(  0  -  15  ] 

1  Sonars 

>  1 

1  ada-1  dac 

1  (  0  -  3  ) 

!  1 

1  1 

ada-l  adc  1 

(  0  -  15  )  1 

<--  Gyros 

dac-2b 
(  0  -  7  ) 


Planes 


This  file  contains  the  functions  which  address  the  A/D  D/A  cards 
directly  in  terms  of  voltages.  */ 


♦  dacl(s,ch)  --  writes  signal  's'  to  ada~l  dac  channel  'ch' 

*  (allowable  channels  0-3) 

void  dad  (s ,  ch) 
int  s,ch; 


if  (NOT_YET_REIMPLEMENTED) 
{ 

ch  =  ch  «  2 ; 


/*  offset  for  G-96  addressing  */ 
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dacl_a [ch]  =  s  »  2 ; 
dacl_a(ch  DAC_LSB_OFFSET] 


S  «  6  ; 


/*  write  upper  8  bits  to  MSB 
/*  write  lower  2  bits  B3,B2 


/ 

/ 


return  ,* 

)  /*  dad  */ 

y»**»*»******»»***»****»****'*-*-**»****-rit*»»****  ****■*********★★♦★*******»**★*★*★ 

*  dac2bfs,ch)  --  writes  signal  's'  to  dac2b  dac  channel  'ch' 

^  (allowable  channels  0-15) 

*-k^*^**ti*-»-K**-k**-Kf'Kt*ir*it**’k*^ciric***t*tc-x**icit*ir*ie1elt********ic*******ic****1c*ie*ir**^ 


void  dac2b ( s, ch ) 
int  s,ch; 

i  f  (NOT_YET_REIMPLEMENrTED) 


ch  =  ch  <<  2;  /*  offset  for  G-96  addressing  */ 

dac2b_aich]  =  s  »  2;  /*  write  upper  8  bits  to  MSB  */ 

.•jadt_aicl:  t>AC_LSB_vOFFSET]  =  s  «  6;  /*  write  lower  2  bits  B3,B2  */ 

] 

return ; 

}  /*  dac2rj  */ 

/•^■i'***-*-K*-r-Ktr***^t*****-Kt!***'lr*****fk*ic'k-kir**-kir*******ic-k*’kic****ir*ii*1e-k***-k*-$eit*icic-k1r 

’  add;rj'  --  reads  ada-1  adc  channel  'n'  (channels  0-15) 

rnt  add  (n i 
int  n  ; 


int  val; 

i f  ( NOT_Y  ET_R  E I M  F  LEMENTED ) 

{ 

add_a  [ADCi_CML_REG]  =  n; 

while  (adcl_a ! ADC1_STATUS_REG j  >  20};  /*  wait  for  data  */ 

val  =  add_a  [ADCl_MSBi  <<  2; 

vai  +=  adcl _ aiADCl _ LSEj  >>  6; 

return  (val); 

}  else  retu r n  ( 0 ) ; 

‘  /’  add  d 

*  adc2(n,g);  --  Reads  adc-2  channel  'n'  (0-15) 

*  with  gain  'g'  (0  to  F  =>  0  -  1024) 

»*»*»»*x»**»***»»*»»*»»»»**»*»*»»*»**», »»**♦*****, *»^^******»»***^*»^^^^^***^y 


int  adc 2 \ n , g ; 
int  n , g ; 

{ 

int  val; 

i f  (NOT_YET_REIMPLEMENTED) 

{ 

adc2_a IADC2_CH_GAIN]  =  (n  «  4)  |  g;  /*  set  c&g,  start  conv  */ 

while ( {adc2_a [ADC2_STATUS_REG]  &  0x7)  !=  0);  /*  wait  for  ready  */ 

/*  This  adc  uses  0  -  4095  to  represent  full  scale  input,  in  order 
to  write  to  the  dac  (which  uses  0  -1023  for  full  scale)  you 
must  divide  val  by  4  or  shift  right  by  2.  Use  the  next  line  to 
get  full  resolution, 
val  =  adc2_a  [ADC2_DATA]  ; 

The  next  line  is  used  for  testing  purposes  only 
val  =  adc2_a[ADC2_DATA]  »  2;  */ 
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val  =  adc2_a  [ADC2_DATA]  ; 
vai  =  val  Sc  OxOFFF; 


)  el 


return ; val )  ; 
se  return  (O'; 
adc2  */ 


/* 


The  prograiT.  code  for  the  Multi-Function  Interface  originated  from  'mfi.c' 
Routines  include  Init_PortA,  Init_PortB,  R€ad_PortA,  Read_PortB,  Read^PortAB 


/ 


Excerpt  of  'mfi.c'  comments  follows: 


Program  example  for  the  Multi-Function- Interface  (MFI) 

This  example  uses  the  6821  PIA  on  the  MFI  board 

General  purpose  functions  are  provided  to  initialize  the  PIA 

and  read/write  data  to  the  ports 


MFI  P2  connector  definitions  are  provided  by  the  GESMFI-1  data 
sheet  available  from  GESPAC,  Inc. 


6821  device  specifics  are  covered  in  the  8-bit  microprocessor 
&  peripheral  data  book  from  Motorola  Inc. 


3/1/91  J.  Rawlins  RealTime  Software  Consulting 


yit»'r**-**»»*»-***»»»**»**»»»*»***»********»****tnt***»*****l>r<**»*<t***»**1t********* 

Init_PortA(base,  dir)  --  Initialize  Port  A  of  MFI 

dir:  1  =  output  port,  0  =  input  port 


void  Irjlt_PortA  {base,  dir) 

register  struct  MFI_PIA  *base;/*  base  address  of  MFI  board  on  G96  bus  */ 

int  dar;  /*  direction:  1  =  outout  port,  0  =  input  port  */ 

{ 

register  short  temp; 

temp  =  base-->cra;  /*  save  contents  of  control  reg .  (no-op)  */ 

Dase->cra  =  0x00;  /*  select:  b2  =  0  data  direction  reg.  */ 

base->pra  =  0x00;  I*  set  portA:  0  =  input  */ 

base->cra  =  0x24;  /»  select:  access  data  reg.s  (b2=l)  */ 

/*  b5=l,b4=0,b3=0 (read  w/cal  restore)  *! 

}/*  Init_PortA  */ 


*  Init_PortB (base,  dir)  --  Initialize  Port  B  of  MFI 

*  dir:  1  =  output  port,  0  =  input  port 

void  Init_PortB  (base,  dir) 


register  struct  MFI_PIA  ♦base;  /*  base  address  of  MFI  board  on  G96  bus  */ 

int  dir;  /*  direction:  1  =  output  base,  0  =  input  base  */ 

{ 

register  short  temp; 

temp  =  (base->crb  &  OxOOFF) ;  /*  get  current  value  of  control  A  ♦/ 

temp  &=  ~4;  /*  clear  bit  #2  so  we  can  access  ddra  ♦/ 

base->crb  =  temp; 

if  (  dir  )  /♦  make  port  B  all  outputs  */ 

base->prb  =  OxOOFF; 

else  /♦  port  B  is  all  inputs  */ 


base->prb  =  0x0000; 

temp  1=  4;  /*  ^2  to  access  data  registers  */ 

base->crb  =  temp; 

Init_PortB  */ 


•^**********’**’kiric*ict****icicicicie**t***t*i[icmet^^i^^ 


Read_PortA  (base)  --  returns  6  bit  value  from  port  A 


igned  char  Read_PortA  (base) 

L'ter  struct  MFI_PIA  »base; 

register  unsigned  short  temp; 
temp  =  base->pra; 
return(temp  &  OxOOFF) ; 


/*  base  address  of  MFI 


/*  read  data  reg. should  reset  busy  */ 
/*  return  data  to  calling  program  */ 


*l****^************'"*****^****************************'^**********ic1,*-t*i:it^ic* 

R^au_PcrtB  'base)  --  returns  8  bit  value  from  port  B 


a  char  Read_PortB  (base) 
r  struct  MF:_PIA  *ba3e; 

register  unsigned  short  temp; 
tem.p  =  base->prb; 
return{temp  Sr  OxOOFF); 


/*  base  address  of  MFI  */ 


*****■>  ^*  ***,,„ 

Reao_PortAB  (base)  --  return  a  16  bit  value  from  ports 
’  A  and  B  combined  then  mas)t  off 

,  Che  15  th  and  16  th  bits. 

Note;  PIA  PA0-PA7  is  the  LSE  and  PB0-PB7  the  MSB 


unsigned  short  Read_PortAB  (base) 

regtstei  struct  MFI_FIA  ’base;  /*  base  address  of  MFI 

1 

register  unsigned  short  hi, lo, temp; 

lo  =  (base->pra  &  OxOOFF);  /*  get  least  significant  byte  from  A 

hi  =  (base->prb  &  OxOOFF);  /*  and  most  significant  byte  from  B 

-emp  =  (ihi  «  8,  +  lo) ;  /*  shift  hi  into  upper  byte  of  word 

return  (  temp  );  /.  return  data 


lr*»<r»******.**^ 


lr**lk»ir****i 


t*******»*******^^*^^ 


void  set_bsyA(base)  /»  sets  CB2  high  (for  busy  to  sending  port) 
register  struct  MFI_PIA  *base;  /*  base  address  of  MFI 
register  short  temp; 

temp  =  (base->cra  &  OxFF) ;  /*  save  era  values 

base->cra  =  0x38;  /*  8  bit  1=  CR2  high 

^  base->cra  =  temp;  /★  restore  era  values 

/*  sets  CB2  low  (for  -busy  to  sending  port)  */ 
void  rst_bsyA (base) 

register  struet  MFI^PIA  *base;  /*  base  address  of  MFI 
register  short  temp; 


62 


cemp  =  (base->cra  &  OxFF) ;  /*  save  era  values  */ 

base->cra  =  0x30;  /*  8  bit  0=  CR2  low  *! 

base->cra  =  temp;  /♦  restore  era  values  *! 

ml  ck_sta  (base) 

register  struct  MFI_PIA  *base;  /*  base  address  of  MFI  */ 

{ 

register  unsigned  short  temp; 

temp  =  base->cra;  /*  save  era  values  */ 

let  urn  (temp) ; 

} 

/***»****★**★★*»»★**★★*♦*♦***»★♦♦♦*★*★*♦★★*★♦*★♦★★★***★****♦♦*★♦*♦****♦***♦♦***/ 

void  center_sonar  () 

{ 

int  direction , encoder_width ; 
char  encode; 

if  (!  SONAR INSTALLED) 

{ 

print f  {" I  scar t /stop  center_sonar ,  SONAR INSTALLED  false] \n"); 
return; 

) 

if  ^TRACE  DISPLAYSCREEN;  printf  (-[start  center_sonar  ()]\n*); 

encoder_width  =  0; 
direction  =  1; 

/*  set_step__si  ze  ( '  H '  )  ;  */  /*  '1'  =  0.9,  '2'  =1.8,  '4'  =  3.6*/ 

Are  we  inside  the  Encoder  Sensor  ?  */ 
encode  =  query _sonar_l_reply  ('M');  /*  Test  Head  Direction  (No  Step)  */ 

if  (SONAPTRACE  DI SPLAYSCREEN) 

print f  :  "center^sonar  :  encode  =  %c\n*' ,  encode)  ; 

if  ((encode  ==  ' t ' )  II  (encode  ==  'T')) 

; 

while  (  (encode  ==  't')  I!  (encode  ==  'T')  ) 

( 

encode  =  query _sonar_l_reply  ('+');  /*  Index  Sonar  direction  */ 

) 

/*  Outside  Encoder  Sensor  Now  */ 

direction  =  -I;  /*  Reverse  Sonar  Rotation  to  Establish  Encoder  Width  */ 

} 

while(  (encode  ==  'f')  I!  (encode  ==  'F')  ) 

{ 

if (direction  ==  1 ) 

{ 

encode  =  query_sonar_l_reply  {'+');  /*  Index  Sonar  '+'  direction  */ 
if  (SONARTRACE  &&  DISPLAYSCREEN)  printf  ( "%c\n*' , encode)  ; 

) 

else 

encode  =  query_sonar_l_reply  {'-');  /*  Index  Sonar  direction  */ 

) 

} 

/*  Found  Edge  of  Encoder  */ 

while{  (encode  ==  't')  II  (encode  ==  'T')  ) 

{ 

encoder_width  =  encoder_width  +  1; 
if (direction  ==  1) 
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{ 

encode  =  query_gonar_l_reply  ('+');  I*  Index  Sonar  direction  */ 

} 

else 

{ 

encode  =  query_sonar_l_reply  /»  Index  Sonar  direction  */ 

} 

) 

if  (SONARTRACE  &&  DISPLAYSCREEN) 

printf  { " cen ter_sonar :  encoder  width  =  %d\n*' ,  encode r_width); 


if  (TRACE  &&  DISPLAYSCREEN )  printf  (“[finish  center_sonar  OJXn"); 
return  ; 

/*  end  center_sonar  i}  »/ 


cha r  q-uery^sona r_l_reply  { comjnand_char } 


ccai'  conuTiand _ char; 


rode  tested  u  taken  from  headtest.c  (prior  version  ahead. c)  */ 

1  :r  t  r  n  d  e  X ,  n ,  r]_b\’ t  e  s  ; 
cnar  reply .xx[20] ,c[l] ; 

If  i!  SONARINSTALLEDJ 

printf  { •' [start/stop  query'_sonar_l_reply  (),  SONAR  INSTALLED  false]  \n-); 
repiy  ; 

return  (reply); 

} 


:r  ;TkACE  £(&  DISPLAYSCREEN )  printf  {“[start  query_sonar_l_reply  ()]\n"); 


/*  likely  string  problems  here 
ciO;  =  command_char ; 


:f  (SONARTRACE  kU  DISPLAYSCREEN} 


printf  f  “quer>'_sonar_l_reply  :  command_char  =  %c\n“,  coininand_char)  ; 
n  =  write  (sonarpath, c, i ) ;  /»  write  characters  to  sonarpath  /t3  device 

tsleep (10) ; 


*/ 


V 


n_bytes  =  _gs_rdy  (sonarpath); 

if  (SONARTRACE  ki.  (n_bytes  >  1)  &&  DISPLAYSCREEN) 

printf  f “query _sonar_l_reply :  lost  reply  data,  n_bytes  =  %d\n“ , n^bytes) ; 

if  (n_bytes  <=  0) 

( 


^  printf (“query_sonar_l_reply:  bad  read,  n^bytes  =  %d\n" , n_ty tes) ; 

else 

{ 

n  =  read  (sonarpath, xx, n_bytes ) ;  /♦  n  unused??????  */ 

if  (SONARTRACE  &&  (n_bytes  >  0)  &&  DISPLAYSCREEN) 

for  {index  =  0;  index  <  n_bytes;  index+4) 

{ 

printf (•t%c  %2d  %2x]  ■,  xx[index),  xx[index],  xxfindex]); 

if  ((index+1)  %  5  ==  0) 

printf ( ‘Vn’ ) ;  /•  prevent  writing  off  screen  */ 
printf {’\n' )  ; 
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] 

reply  =  xx [0 • ; 

if  (TRACE  &&  DISPLAYSCREEN) 

printf  (M finish  guery_sonar_l_reply  ()  returns  %c]\n'‘,  reply); 
return  (reply); 

} 

/»***★*  ★»*****************.******<:**»*********»^*^t********^^**************»***^**^ 

void  set_step_size  (step_code) 
char  step_code; 

1 

unsigned  int  n; 
char  reply; 

if  (!  SONAR INSTALLED) 

printf  ( - (start/stop  set_step_size  (),  SONAR INSTALLED  false] \n"); 
return ; 


if  (TRACE  USc  DISPLAYSCREEN)  printf  {"[start  set_step_si ze  ()]\n"); 

if  (SCKARTRACE  DISPLAYSCREEN)  printf  ("step  code  =  %c\n" ,  step_code)  ; 
write  (sonarpath, step_code, 1 ) ; 
tsieep  C);  /*  2S6ths  of  a  second  »/ 

n  =  read  (scnarpath,  reply,  1);  /*  n  unused??????  */ 

if  (SOKARTPJiCE  DISPLAYSCREEN)  printf  {"step  =  %c\n ",  reply )  ; 
if  {TRACE  &&  DISPLAYSCREEN)  printf  ("[finish  set_step_si ze  ()]\n"); 
return; 


/***-^^^^*^*-r***********’k*****t********^*-k^t**tr-k*t*****ir*-k**ir*icifk*ir*^ir***1t*itit*ic*^ 

void  tty_Tr.ode  ( tty_iBode_path,  mode) 
int  tty_mode_path; 

int  mode;  /*  note  type  specifications  differ  from  headtest.c  */ 

{ 

static  struct  sgbuf  old, new; 
static  int  init  =  1; 
int  status; 

if  {]  SONARINSTALLED) 

{ 

if  (TRACE  &&  DISPLAYSCREEN) 

printf  ("[start/stop  tty_mode,  SONARINSTALLED  false  {)]\n*); 
return; 

) 

if  (init) 

{ 

if  (TRACE  fif&  DISPLAYSCREEN)  printf  ("[start  tty^mode  ()]\n"); 
init  =  0; 

status  =  _gs_opt  (tty_inode_path,  &old)  ; 
status  =  gs,  opt ( tty^mode _path,  &new) ; 

new.sg_class  =  0; 
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new . sg_case 

s= 

0 

new . sg_backsp 

= 

0 

new . sg_delete 

0 

new . sg_echo 

= 

0 

new . sg_al f 

= 

0 

new.sg__nulls 

= 

0 

new . sg_pause 

= 

0 

new.sg_page 

= 

0 

new . sg_bspch 

= 

0 

new . sg_dlnch 

0 

new . sg_ecrch 

c 

•  sg_eof  ch 

= 

0 

new. sg_r Inch 

0 

new . sg_dulnch 

= 

0 

new . sg_psch 

= 

0 

new . sg_kb: ch 

= 

0 

new . sg_kbach 

= 

c 

new . sg_bsech 

= 

0 

new . sg_bellch 

0, 

new . sg_pari ty 

0, 

new . sg_L abcr 

0, 

n  ew . s  g_t  a  bs i z 

= 

0, 

new . sg_tbl 

r 

0; 

new . sg^ccl 

= 

0. 

new . sg^err 

= 

0; 

if  (mode;  _ss_opt  ( tty_inode_path,  Scnew)  ; 

-ss^opt  (tty_modG_path,  &old) ; 

if  (TPACE  SiU  DISPLAYSCREEN)  printf  {“[finish  tty_mode  ()]\n“}; 
return; 

} 


/**»»**»»**-xlr**x***»*»»*-r»*» 

void  print_vaIid_keywords  {} 


-»*****»*★**♦****★*»*»***. ! 


prinrf 

prinrf  - 
printf  (“ 
printf  (" 
printf  (“ 

prinrf  i '* 
printf  ' 
printf  {“ 
printf  (“ 
printf  (“See 
printf 


[help]  [ trace ! notrace]  [loopf orever I looponce] \n " } ; 
[wait  #]  [time  #]  [timestep  (0.0.. 5.0)]  [mission] \n“ ) ; 
[keyboard! keyboard- off]  [quit]  [kill] \n") ; 

[rpm]  [course]  [depth]  [thrusters  I thrusters-off ] \n“ ) ; 
[loopfilebackup]  [ entercontrol constants ] \n“); 

[rotate]  [position]  [orientation] \n“); 

[sonartrace isonartraceoff ]  [sonarinstalled] \n“) ; 
[trace! trace-off]  [parallelporttrace]  \n" ) ; 

[remotehost  hostname]  [realtime  I  nopause]  [pause] \n“); 
[loop-forever  I loop-once] [entercontrolconstants] \n\n“ )  ; 
[silence] [e-mail  I  no-email ]  [waypoint]\n\n“); 
/execution/mission. script. HELP  for  command  syntax  details .\n“); 


#if  defined(sgi) 

printf  (“popping  up  'mission . script .HELP'  as  a  zip  file..,\n“); 
system  (“zip  -v  -/execution/mission. script .HELP“ ) ; 

#endi  f 


return; 


/*  end  print_valid_keywords  */ 


void  open_virtual_world_socket  () 


/*  see  os9sender.c  for  original  code  */ 


if  (TRACE  &&  DISPLAYSCREEN) 

printf  ("[start  open_virtual_world_socket  ()]\n"); 


/*  Initialize  communications  blocks 
/*  Initialize  both  client  &  server  ' 


/*  Signal  handlers  for  termination  to  override  net^open  {)  and  net_c lose  {)*/ 
/»  ’signal  handlers.  Otherwise  you  are  unable  to  '"C  kill  this  program.  / 

#if  defined -vsgi  } 

signal  (SIGHUP,  shutdown_virtual_world_socket) ;/♦  hangup  J 

signal  (SIGINT,  shutdown_virtual_world_socket ) ; /*  interrupt  character^  / 

signal  (SIGKILL,  shutdown_virtual_world_socket ) ; /*  kill  signal  from  Unix  / 

signal  (SIGFTPE,  shutdown_virtual_world_socket ) ; /♦  broken  pipe  from  other  host  / 

signal  (SIGTERM,  shutdown_virtual_world_socket ) ; /*  software  termination  / 

#enda  f 

/*  initialize  sender 

/*  start  by  finding  def aul t/desired  remote  host  to  connect  to  */ 

{ 

server_enti ty  =  gethostbyname  (virtual_world_remote_host_name) ; 
if  (server_enti ty  ==  NULL) 

if  (DISPLAYSCREEN) 

printf (" [open_virtual_world_socket :  virtual  world  remote  hostXn"); 
printf{"  (\"%s\")  not  foundjXn", 

virtual _wo  r 1 d_r emo  t  e_hos  t_name ) ; 
fflush  (stdout); 

/»  error  message  needed  on  (open)  output  file  <<<<<<<<<<<<<<<<<<<<  *I 
vi rtual_world_socket_opened  =  FALSE; 
return ; 

else  if  fTRACE  &&  DISPLAYSCREEN) 

printf {" [open_virtual_world_socket:  virtual  world  remote  host 
printf ( " ( \"%s\" )  located) \n" ,  virtual_world_remote_host_name) ; 


/*  Client  opens  server  port  V 

/*  Fill  in  structure  ' server_address '  with  the  address  of  the  */ 

remote  host  (i.e.  SERVER)  that  we  want  to  connect  with: 

#if  definedisgi)  .  ^ ^ 

bzero  ((char  *)  &server_address ,  sizeof  (server_aaaress ) ) ; 

^^^'^^^server_address.sin_family  =  AF^INET;  /*  Internet  protocol  family  */ 

/*  copy  server  IP  address  into  sockaddr_in  struct  serve readdress  */ 

#if  defined (sgi)  , 

bcopy  (server_entity->h_addr,  & (server_address . sin_addr.s_addr) , 

server_entity->h„length) ; 

#el  se 

strncpy  {&  (server_address.sin_addr.s_addr) ,  server_entity->h_addr, 
server_entity->h_length) ; 

#endif 

/*  make  sure  port  is  in  network  byte  order  */ 

g0rver_addres s .  sin_port  =  htons  (AUVSIM1^TCP^P0RT_1 ) ; 

/*  Open  TCP  (Internet  stream)  socket 


/ 


if  (  (socket_descriptor  =  socket  (AF_INET,  SOCK_STREAM,  0))  <  0  ) 

if  (DISPLAYSCREEN) 

{ 

printf  f  *' [  open_Yirtual_world_socket :  client  can't  open  server") 
printf  ("  virtual  world  stream  socket]"); 

} 

/*  error  message  needed  on  (open)  output  file  <<<<<<<<<<<<<<<<<<<<<< 
vi rtual_world_socket_opened  =  FALSE; 
return ; 

) 

else  if  (TRACE  &&  DISPLAYSCREEN) 

{ 

printf  { " [ open_virtual_world_socket :  client  opened"); 

^  printf  (*  virtual  world  server  socket  successfully ] \n ") ; 

/*  Connect  to  the  server.  Process  will  block/sleep  until  connection  is 
is  established.  Timeout  will  return  an  error, 
if  (connect  (  socket_descriptor, 

(struct  sockaddr  *)  &server_address , 

sizecf  (server_address) )  <  0) 

if  (DISPLAYSCREEN) 

{ 

printf  ( " [ open_virtual_world_socket :  client  can't  connect  to"); 
printf  ("  virtual  world  server  socket] \n"); 


/ ■"  error  message  needed  on  (open)  output  file 
VI rtuai_world_socket_opened  =  FALSE; 


else  if  (TRACE  &&  DISPLAYSCREEN) 

printf  ("{execution  client  connected  to  virtual  world"); 
printf  {"  server  socket  successfully ] \n ") ; 

( 

VI rtual_world_socket_opened  =  TRUE; 

:  /'  end  initialization 


socket_stream  =  socket_descriptor ; 


/*  client  */ 


If  (TRACE  &S,  DISPLAYSCREEN)  /*  print  final  info  V 


printf ;■ [open_virtual_world_sockec  CLIENT:  socket_descriptor  =  %d]\nV 

.  ^  f  ,  socket_descriptor) ; 

'  I  socket_accepted  =  %d)\nV 

socket_accepted) ; 

printf  i"[  socket_streani  =  %d]\n", 

socket_streain) ; 


if  (TRACE  DISPLAYSCREEN) 

printf  ("[finish  open_virtual_world_socket  {)]\n' 

return ; 

)/*  end  open_virtual_world_socket  ()  */ 


void  shutdovm_virtual_world_socket  ()  /•  see  os9sender.c  for  original  code  •/ 

int  kill_return_value; 
shutdown_signal_received  =  TRUE; 
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if  (virtual_world_socket_opened  ==  FALSE) 

if  (TRACE  &&  DISPLAYSCREEN) 

{ 

printf  (- [virtual_world_socket_opened  FALSE, " ) ; 
printf  ( **  shutdowri_virtuaI_worid_socket  ignored]  \n* ) ; 

} 

return; 

) 

if  (TRACE  DISPLAYSCREEN) 

printf  ( " [ shutdown_virtual_world_socket  start  ...jNn"); 

/*  No  need  to  send  a  message  to  other  side  that  bridge  is  going  dovm,  */ 

/*  since  SIGPIPE  signal  trigger  may  shutdown  server  on  other  side  */ 

if  (close  (socket_st ream)  ==  -1) 

if  (TRACE  DISPLAYSCREEN) 

printf  ( *'shutdown_virtuai_world_socket  close  (socket_stream)  failedNn"); 

/*  shutdown  ()  reference:  'Using  OS-9  Internet*  manual  p.  2-55  */ 

if  (shutdown  { socket_stream,  2)  ==  -1) 

1 

If  (TRACE  DISPLAYSCREEN) 

printf  ( " [ shutdown_virtual_world_socket  shutdown*); 
printf  ("  (socket_stream,  2)  failed] \n"); 


kill_return_value  =  kill  (socket_stream,  SIGKILL) ; 
if  (TRACE  fic&  DISPLAYSCREEN) 

printf  ( * [shutdown_virtual_world_socket  kill  ( socket_stream, " ) ; 
printf  f*  SIGKILL)  returned  %d]\n*,  kill_return_value) ; 


if  (TRACE  LL  DISPLAYSCREEN) 

printf  ( "  [shiUtdown_virtual_world_socket  return]  \n" )  ; 

return ; 

}  /*  end  shutdowri_virtual_worid_socket  ()  */ 

/*-r-*->**-^-f*'t**^****-K*****i:*’K*******-,t*****t:ir*’k**ic**-k**it**1fk***it*ie**itie**1c***ic*icitfk^ 

void  send_d3ta_on_virtuaI_world_socket  ()/*  see  os9sender.c  for  original  code  */ 
i 

bytes_left  =  socket_length; 

by tes_written  =  0; 

ptr_index  =  buffer;  /*  this  global  string  is  the  data  to  be  sent  */ 

if  (virtual_world_socket_opened  ==  FALSE) 

{ 

return; 

} 

if  (TRACE  &&  DISPLAYSCREEN) 

printf  { * [send_data_on_virtual_world_socket  start  ...]\n*); 

while  { (bytes_lef t  >  0)  &&  {bytes_written  >=  0))  /*  write  loop  ***********/ 

bytes_sent  =  write  {socket_stream,  ptr_index,  l^tes^lef t ) ; 

if  (bytes_sent  <  0)  bytes_written  =  bytes_sent; 

else  if  (bytes_sent  >  0) 
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t>ytes_lefc  -=  bytes_sent; 
by  tes_wri  tten  +=  bytes_sent; 
ptr_index  +=  bytes_sent; 


OCATIONLAE  UU  TRACE  DI SPLAYSCREEN) 

printf  { *  [  record_data  send_teleinetry_to_server  loop* 
printf  ( "  bytes  sent  =  %d)\n’‘,  bytes_sent); 


if  {by tes_wri tten  <  0) 

i 

if  (LOCATIONLAB  USc  DISPLAYSCREEN) 

{ 

printf  ( "  [  record_data  send_t el eTnetry_t observer  ()  send  failed,  " )  ; 

^  printf  (**%d  by tes_wri tten]  \n * ,  bytes_written)  ; 

/*  error  message  needed  on  (open)  output  file  <<<<<<<<<<<<<<<<<<<<<< 

i 

else  if  (LOCATIONLAB  &&  TRACE  &&  DISPLAYSCREEN) 
i 

printf  { " [ record_da ta  send_telemetry_t observer  total  bytes  sent") 
printf  {"  =  %d]\n“,  bytes_wri tten) ; 


*  Cr:eck  termi na ti on  ***»****■***»****»<*♦»»****»*»***★*»***♦»»*★****»★*★★*★*/ 

if  fstrncmp  (buffer,  "shutdown",  8)  ==  0) 

t 

If  (TRACE  &&  DISPLAYSCREEN)  printf 

' " 1 send^dat a_on__vi rtual_world_socket :  shutdown_signal_received) \n") ; 
shutdown^vir tua2_wcrid_socket  ()  ; 

::  (TRACE  DISPLAYSCREEN; 

printf  , “ [ send_data_on_virtual_world_socket  return] \n") ; 

/*  end  send_data_cn_virtuai_worid_socket  { )  */ 


*****ii****i,**i,*it*<t**ititieir*i,**ir*-i 


void  get_stream_f rorruvirtuaI_worid_socket  ()  /*  see  os9sender.c  for  original  */ 

if  ( virtuai_world_socket_opened  ==  FALSE) 

{ 

return; 

! 

if  (TRACE  &&  DISPLAYSCREEN) 

printf  ( " [get^stream^f rom_virtual_world_socket  start  .,.]\n"); 

/*  listen  to  remote  host,  relay  to  local  network/program  */ 


bytes_left  =  socket_length; 

bytes_received  =  0; 

ptr_index  =  buf fer^received; 


while  ( (bytes_left  >  0)  &&  (by tes_received  >=  0))  /*  read  loop  **♦*********/ 

by^^es_read  =  read  (socket_stream,  ptr_index,  bytes^left); 

if  (bytes_read  <  0)  bytes_received  =  bytes_read; 

else  if  (bytes_read  >  0) 

{ 

bytes_left  ~=  bytes_read; 
by te s_ received  +=  byte spread; 
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ptr_index  +=  bytes^read; 

) 

if  (TRACE  DISPLAYSCREEN) 

{ 

printf  ( “ [get_stream_f rom_virtual_world_socket  receiver  block"}; 
printf  ("  loop  bytes_read  =  %d]\n",  bytes^read) ; 

} 

/*  if  nothing  is  waiting  to  be  read,  break  out  of  read  loop  */ 

if  (  (bytes_read  ==  0)  (bytes_received  ==  0))  break; 

} 

if  (bytes_received  <  0)  /*  failure  */ 

\ 

if  (TRACE  DISPLAYSCREEN) 

{ 

printf  ( "  [get_streain_f  roir,_virtual_world_socket  receiver  block  read"}; 
printf  ("  failed,  by tes_received  =  %d\n",  by tes_received) ; 

I  ^ 

else  if  {bytes_recei ved  ==  0)  /*  no  transfer  */ 

if  (TRACE  ixU  DISPLAYSCREEN) 

print:  ;  *'  [get_strearri_f  rom_virtual_world_socket  received  0  bytes!  !  ]  \n")  ; 

} 

else  if  (by te£_recei ved  >  0)  /*  success  */ 

t 

if  (TRACE  kU  DISPLAYSCREEN) 

1 

print f  (*■  [get_streair._froiTi_virtual_world_socket  received  %d  bytes]\n", 
by  tes_received ,)  ; 

) 

) 

/*  Check  teinr.i nation  **'^***************^***********************************^/ 

if  (strncrr^p  :buf f er_received,  "shutdown",  8)  ==  0} 

{ 

if  (TRACE  &&  DISPLAYSCREEN)  printf 

( " (get_data_on_virtuai_world_socket :  shutdown_signal_received] \n" ) ; 
sriUtdown_virtual_world_socket  ()  ; 

if  (TRACE  &&  DISPLAYSCREEN) 

printf  (*  [get_streait_f  roin_virtual_world_socket  return]  \n")  ; 
return ; 

}  /*  end  get_strearr!_f roin_virtual_world_socket  ()  */ 


double  degrees  (rads)  /*  radians  input  */ 

double  rads; 

( 

return  rads  *  180.0  /  PI; 

} 

double  radians  (degs)  /*  degrees  input*/ 

double  degs; 

{ 

return  degs  ♦  PI  /  180.0; 

)**** 

double  normalize  (degs)  /*  degrees  input*/ 
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double  degs; 


dcubie  result  =  degs; 

while  (result  <  0.0)  result  +=  360.0; 

wr.:]e  (result  >=  360.0)  result  -=  360.0; 

return  result; 


double  normalize2  (degs)  /*  degrees  input*/ 
douDle  degs; 

{ 

dcubie  result  =  degs; 

while  (result  <-=  -180.0)  result  +=  360.0; 
wnlle  (result  >  180.0)  result  -=  560.0; 

return  result; 


double  radian^noriTtalize  (rads)  /*  radians  input*/ 

double  rads; 

{ 

dcubie  result  =  rads; 

while  (result  <  0.0)  result  +=  2.0  *  PI; 

while  (result  >=  2.0  *  PI)  result  -=  2.0  *  PI; 

return  result; 


t*******»»***i 


dcubie  radian^ncriTial i  ze2  (rads) 
dcubie  rads; 

{ 

dcubie  result  =  rads; 


/*  radians  input*/ 


while  (result  <=  -  PI)  result  +=  2.0  *  PI; 
while  (result  >  PI)  result  -=  2.0  *  PI; 


return  ret 


r»»*^t*'»»***'*^*ltit*****^ 


t***^****s>r*****lt1(r**-J 


void  clamp  (clainpee,  absolute_inin ,  absolute^max ,  name) 
double  claiTipee; 
double  absolute_min; 
double  absolute_inax; 
char  *  name; 

{ 

double  n€w_value,  Iocal_min,  local_max; 


if  ( (absolute_max  ==  0.0)  &&  {absolute_min  ==  0.0))  return;  /*  no  clamp 

if  {absolute_max  >=  absolutejiin)  /*  ensure  min  &  max  used  in  proper  order 

local_min  =  absolute_min; 
local_max  =  absolute_max; 


local^min  =  absolute_max; 
local_max  =  absolute_min; 
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if  (  '■*  claiDpee)  >  local^max) 

{ 

new^value  =  local_rrtax; 

it  (TRACE  DISPLAYSCREEN) 

printf  (- [clamping  %s  from  %5.3f  to  %5.3f]\n“, 
name,  *  clampee,  new_value) ; 

*  clampee  =  new_value; 

} 

if  (  (*  clampee)  -r  local_min) 

new_value  =  local_min; 

if  (TRACE  UU  DISPLAYSCREEN) 

printf  (Mclamping  %s  from  %5.3f  to  %5,3f]\n\ 
name,  *  clampee,  new_value) ; 

*  clampee  =  new_value; 


**»★»****•*»♦■**»*******, *******T(^***.»**»inr*»**^*'**»***y 


’  tnaiiKs  to  Michael  Olberg  Oct  20,  94  olberg@bele.oso.chalmers.se  */ 

docble  atanSiy,  x ; 

double  y;  douLle  x; 

If  f TRACE  UU  DISPLAYSCREEN) 

printf  (Matan2  {%5.3f,  %5.3f)]\n-,  y,  x); 

if  'x  ==  G.o:  { 

if  ;y  •'  0.0;  return  i-FI/2 .0)  ; 

else  return;  PI/2.0); 

}  else  { 

if  (X  <r  0.0)  { 

if  (y  <  0.0)  return  (atan  (y/x) -PI )  ; 
else  return (a tan (y/x) +PI } ; 

»  else  return  (atari  (y/x)  }  ; 

I 

as  to  the  tanh  you  will  simply  have  to  use  */ 

double  3inh{x) 

double  X; 

{ 

return  (exp{x)  -  exp ( -x) ) /2 . 0 ; 

} 

double  cosh(x) 

double  X; 

return  (exp(x}  +  €xp(-x) ) /2 .0; 

} 


double  tanh(x) 

double  X; 

{ 

return  sinh (x) /cosh (x) ; 

} 

#endi  f 
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double  sign  (x) 
double  X; 
i 

if  (X  >  0.0)  return  1,0; 

else  if  (x  <  0.0)  return  -1.0; 

else  return  0.0; 


/ *  end  cf  execu  1 1  on . c  * / 
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D. 


parse _functions.c  Tactical  Script  Command  Parse  Functions 


/’ 

Program : 

Authors : 

Revised : 

System : 
Compiler : 

Compilation : 

[66020] 

(68030] 

[Irix  ] 


parse_funct ions . c 
Don  Brutzman 
28  October  94 

AUV  Gespac  68020/68030,  OS-9  version  2.4 
Gespac  cc  Kernighan  &  Richie  (K&R)  C 

ftp>  put  parse_functions . c 

auYsiml>  chd  execution 
auvsiml>  make  -k2f  execution 
auvsiml>  make  execution 

fletch>  make  execution 

Reduce  size  of  execution. c  to  allow  OS-9  C  compiler  to  work 


/*  parse_functions . c  */ 

#inc]ude  “globals  .h*' 

# include  "def  ines  . h 

0  ; 
0  ; 
0  ; 


VC  I  a 
veld 
void 


parse_c ommand_l ine_f lags 
parse_missi on_script_commands 
parse_mission_string_coinmands 


*  *  *  / 


extern  void 
extern  void 
extern  void 
extern  void 


print_valid_key words  (); 
send_data_on_vi rtuai_world_socket  () ; 
get_control_constants  (); 
clamp  (); 


/■»»»»»*-r»*»*»***»»5r»»****»********r*Tr******»***-************int»*****<r***^^***.»***»  j 


veld  parse_coTnmand_line_f lags  {arge,  argv) 

int  arge;  char  **argv;  /♦  command  line  arguments  *! 

{ 

int  index; 

if  (DISPLAYSCREEN) 

{ 

printf  ( •  [parse_coiTimand_line_f  lags  start:  #  arguments  =  %d]\nl*,  arge); 
for  (i  =  0;  i  <  arge;  printf  (•  %s“,  argv[i]); 

printf  { "  JNn*); 

) 

if  (DISPLAYSCREEN)  printf  {“ (parse  arguments :  "); 

f 

for  (i  =  1;  i  <  arge;  i++) 

{ 

printf  ("%s  •,  argv[i]); 

for  (index  =  0;  index  <=  {int)strlen  (argvii]);  index++)/*  uppercase  */ 
argv[i]  [index]  =  toupper  (argv[i]  [index]); 

} 

printf  (NXn*); 
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strcpy  (buffer,  ■ " ) ;  /*  initialize  for  SILENT  */ 
for  (i  =  1;  i  <  argc;  i++) 

if  ((strcmp  (argv[i],  ‘HELP’)  ==  0)  I  I 

(strcmp  (argv[i],  •?•)  ==  O)  II 

(strcmp  (argvii),  ■/?■)  ==  0)  II 

(strcmp  (argvii),  ==  O) ) 

if  (TRACE  &&  DISPLAYSCREEN)  printf  ( • [print_help]  “); 

^  print_help  =  TRUE; 

else  if  ((strcmp  (argv[i],  "KEYBOARD")  ==  0)  II 

(strcmp  (argvii),  "KEY-BOARD")  ==  0)  II 

(strcmp  (argvii),  "KEYBOARD- INPUT" )  ==  0)  II 
(strcmp  (argvii),  "KEYBOARDINPUT" )  ==  0)) 

{ 

if  (TRACE  DISPLAYSCREEN)  printf  ("[KEYBOARDINPUT  =  TRUE)  " ) : 

KEYBOARDINPUT  =  TRUE; 

) 

else  if  (strcmp  (argv'i),  "TRACE")  ==  0) 

{ 

if  (TRACE  StS.  DISPLAYSCREEN)  printf  ("[TRACE  =  TRUE]  " )  ; 

TRACE  =  TRUE; 


if  {  {scrcKip 

(argv | 

[i]  , 

"TRACEOFF") 

=  =  0) 

(strcmp 

( a  rg V  | 

[i]  , 

•TRACE-OFF" ) 

=  =  0) 

(strcmp 

(argvi 

[i] , 

■NOTRACE") 

=  =  0) 

(strcmp 

(argvl 

li] , 

■NO-TRACE") 

=  =  0) 

if  (iRAv-E  &&  DISPLAYSCREEN)  printf  ("[TRACE  =  FALSEl  "); 

TRACE  =  FALSE; 

} 

else  if  ({strcmp  (argv[i),  "LOOPFOREVER" )  ==  0)  II 
(strcmp  (argvii),  " LOOP- FOREVER ' )  ==  0)) 

if  (TRACE  ill,  DISPLAYSCREEN)  printf  ("[LOOPFOREVER]  ")• 
LOOPFOREVER  =  TRUE; 

.f 

else  if  ((strcmp  (argv[i],  "LOOPONCE")  ==  0)  l| 

(strcmp  (argv[i),  "LOOP-ONCE")  ==  0)) 

if  (TRACE  £.&  DISPLAYSCREEN)  printf  ("[LOOPONCE)  ")• 

LOOPFOREVER  =  FALSE; 

/ 

else  if  ((strcmp  (argv[i],  "LOOPFILEBACKUP" )  ==  0 )  II 

^  (strcmp  (argvii),  "LOOP-FILE-BACKUP")  ==  O)) 

if  (TRACE  &&  DISPLAYSCREEN)  printf  ("[LOOPFILEBACKUP!  ")• 
LOOPFILEBACKUP  =  TRUE; 

} 

else  if  ((strcmp  (argv[i],  "ENTERCONTROLCONSTANTS" )  ==  0)  || 

^  (strcmp  (argv[i],  "ENTER-CONTROL-CONSTANTS" )  ==  0)) 

DISPLAYSCREEN)  printf  ("[ENTERCONTROLCONSTANTS]  "); 
ENTERCONTROLCONSTANTS  =  TRUE;  wwixc  i  a  j  /, 

) 

else  if  ((strcmp  (argv[i],  "TACTICAL")  ==  0)  || 

(strcmp  (argv[i],  "TACTICAL-ON")  ==  0)) 

{ 

printf  ("[%s)\n",  argv(i]); 

TACTICAL  =  TRUE; 

} 

else  if  ((strcmp  (argv[i],  "NO-TACTICAL" )  ==  0)  || 

(strcmp  (argv[i),  " TACT I CAL- OFF " )  ==  0)) 
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printf  (•‘[%s]\n",  argv[i]); 

TACTICAL  =  FALSE; 

} 

else  if  ( (strcmp  {argvfi],  • SONARTRACE " )  ==  0)  II 
(strciDp  (argviii,  "SONAR-TRACE" )  ==  0)) 

^  if  (TRACE  DISPLAYSCREEN)  printf  {"[SONARTRACE]  *); 

SONARTRACE  =  TRUE; 

else  if  ( (strcmp  (argv[i],  "SONARTRACEOFF" )  ==  0)  II 

(strcinp  {argvii],  " SONAR -TRACE- OFF " )  ==  0)) 

^  if  (TRACE  ScSc  DISPLAYSCREEN)  printf  ("[SONARTRACEOFF]  *); 

SONARTRACE  =  FALSE; 

else  if  ( (strcmp  (argv[il/  "SONARINSTALLED" )  ==  0)  II 
(strcmp  (argvii]/  "SONAR- INSTALLED" )  =-  0)) 

if  (TRACE  &&  DISPLAYSCREEN)  printf  ("[SONARINSTALLED]  "); 
SONARINSTALLED  =  TRUE; 

else  if  ( {strcmp  (argv[i]/  " PARALLELPORTTRACE " )  ==  0)  II 
(strcmp  (argv[i],  " PARALLEL- PORT -TRACE " )  ==  0)) 

if  (TRACE  DISPLAYSCREEN)  printf  ("[PARALLELPORTTRACE]  "); 
PARALLELPORTTRACE  =  TRUE ; 

=e  if  '(strcmp  (arqv[i],  "SILEITT")  ==  0)  II 
(strcmp  (argvIi]/  "SILENCE")  ==  0)) 

if  (TRACE  US.  DISPLAYSCREEN)  printf  ("[SILENT]  "); 

/*  send  to  virtual  world  after  socket  is  open  */ 

strcpy  (buffer/  "SILENT");  /*  copy  current  command  to  buffer  */ 

else  if  ({strcmp  {argv[i]/  "TIMESTEP")  ==  0)  M 
{strcmp  (argvii]/  "TIME-STEP")  ==  0)) 
i 

if  (i  >=  argc)  print_help  =  TRUE; 
else 

sscanf  (argvii),  •%?“,  &TIMESTEP) ; 

if  (TRACE  &&  DISPLAYSCREEN)  printf C [TIMESTEP  %f]',  TIMESTEP); 
if  (TIMESTEP  >  C.O)  dt  =  TIMESTEP; 
else  if  (TRACE  &&  DISPLAYSCREEN) 

printfC  illegal  TIMESTEP  value,  ignored.*); 
if  (TRUE  DISPLAYSCREEN)  printfC  [dt  =  %f]*,  dt); 

) 

else  if  ({strcmp  (argvii),  * REMOTEHOST “ )  ==  0)  II 
(strcmp  (argvii),  * REMOTE- HOST “ )  ==  0)  II 
(strcmp  (argvii),  "REMOTE*)  ==  0)  II 

(strcmp  (argvii),  "HOST")  ==  0)) 

{ 

i  +  + ; 

if  (i  >=  argc)  print_help  =  TRUE; 
else 

^  sscanf  (argv[i],  •%s",  virtual_world_reinote_host_name) ; 
if  (TRACE  DISPLAYSCREEN) 

printf ( • [REMOTE-HOST  %s] “ ,  virtual_world_remote_host_name) 

) 

else  if  ((strcmp  (argvii],  "REALTIME")  ==  0)  II 
(strcmp  (argvii}/  "REAL-TIME")  ==  0)) 

( 


if  {TRACE  &&  DISPLAYSCREEN)  printf  (-[REALTIME]  “ ) ; 

REALTIME  =  TRUE; 

} 

else  if  ( (strcmp  (argvji],  -NOREALTIME")  ==  0)  11 

(strcinp  (argv[i],  "NO-REALTIME")  ==0)  II 

(strcmp  (argviij,  "NO-REAL-TIME"-}  ==  0)  M 

(strcmp  (argv[i],  "NOWAIT")  ==  0)  11 

(strcmp  (argv[i],  "NO-WAIT")  ==0)  II 

(strcmp  (argvfij,  "NOPAUSE")  ==  0)  II 

(strcmp  (argvji ],  "NO- PAUSE")  ==  0)) 

if  (TRACE  DISPLAYSCREEN)  printf  (“[NOWAIT]  "}; 

REALTIME  =  FALSE; 

) 

else  if  ((strcm.p  (argv[i],  "NOEMAIL")  ==  0)  II 
(strcmp  (argvji],  "NO-EMAIL")  ==  0)) 

j 

if  (TRACE  DISPLAYSCREEN)  printf  ("[NO  EMAIL]  “); 

EMAIL  =  FALSE; 


else  print_h6lp  =  TRUE;  /*  invalid  command  line  entry  parameter  found  */ 
/*  end  for  loop  through  command  line  paramieters  */ 


11  ;print_heip)  /*  print  help  string 
{ 

p r i n t  f ( " \ nU sa ge :  execu  t i on  \ n " ) ; 
print_val id_keywords  () ; 
exit  [-1 . ; 


if  (TRACE  kl  DISPLAYSCREEN)  printf  ( " \n [parse_command_line_f lags  complete] \n" ) 
return ; 


i  /’  end  parse_coiTmand_line_f lags  {)  */ 

/•K-»-rir»->rlr»»»-r'*''r»-*-)r»-»**'*-'»*»*'r*-*****-*-**--**-»*Tr-»*->e**»*»*'*-*-^-*--»*********** 


/ 


v 

{ 


d  pai'se_m.is£icn_script_commands  ()  get  data  from,  file  at  program  start  */ 

/*  mission . script . HELP  =>  descriptions  */ 


:nt  index,  read_another_l ine,  parameters_read; 
dour.ie  parameterl ,  parameter2,  parameters,  parameter4,  parameters; 
char  backupcommand  [SO],  new_filename  [30]; 


if  (TRACE  &&  DISPLAYSCREEN) 

printf  {"[start  parse_mi ssion_script_coinmands  ()]\n"); 

if  ( (auvscriptf ile  ==  NULL)  M  feof  (auvscriptf ile )  II  auvscriptf ilequit ) 
( 

if  (DISPLAYSCREEN) 

{ 

printf  ("[opening  a  copy  of  the  auvscriptf ile  %s]\n", 
AUVSCRIPTFILENAME) ; 
fflush  (stdout); 

} 

#if  defined (sgi) 

sprintf  (backupcommand,  “rm  mission. script .backup" ) ; 

printf  {-%s\n-,  backupcommand); 
system  (backupcommand) ; 

sprintf  (backupcommand,  “cp  %s  mission. script .backup" , 
AUVSCRIPTFILENAME) ; 
printf  ( “%s\n“ ,  backupcommand) ; 
system  (backupcommand) ; 

4felse 

/*  OS-9  */ 
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sprincf  fbackupcoirunand,  “del  mission .  script  .backup" )  ; 

prinrf  {•%s\n’',  backupcoinmand)  ; 
system  fbackupcommand) ; 

sprintf  (backupcommand,  “copy  %s  mission . script .backup" , 
AUVSCRIPTFILENAl^E)  ; 
printf  (•%s\n“,  backupcommand) ; 
system  (backupcommand) ; 


auvscriptf ile  =  fopen  ( “mission . script .backup* , “r" ) ;  /♦  input  file  */ 

if  (auvscriptf ile  ==  NULL) 

{ 

printf  {“AUV  execution:  script  file  %s\n“,  AUVSCRIPTFILENAME) ; 
printf 

( “  (or  backup  copy  mission. script .backup)  not  found. \n“) 

printf  f“  Ensure  you  are  in  the  right  directory : \n“ ) 

printf  ("  auvsiml>  chd  /hO/execution  or\n“); 

printf  (“  unix>  cd  ~brutzman/execution\n“ ) ; 

printf  { “Exit . \n* } ; 
exit  (-1); 

} 

auvscriptf ilequit  =  FALSE; 

sprintf  (buffer,  “%s. backup",  AUVORDERSFILENAME) ; 
auvordersfile  =  fopen  (buf f er , “w“ ) ;  /*  output  file  */ 

if  (TRACE  &&  DISPLAYSCREEN) 

printf  (“auvordersfile  (%s)  =  %x\n*,  AUVORDERSFILENAME, 
auvordersfile) ; 
if  (auvordersfile  ==  NULL) 

( 

print f  ( "AUV  execution:  %s  file  not  opened. \n",  buffer); 
printf  1"  Error -Xn"); 

printf  ( “Exi t . \n " ) ; 
exit  ( - 1 ) ; 

} 

if  (TRACE  DISPLAYSCREEN) 

printf  (“[auvordersfile  =  %x,  opened  succesfully ] \n“ , 
auvordersfile) ; 

fprintf  (auvordersfile, 


fprintf  (auvordersfile, 

NPS  AUV  file  %s:  commanded  propulsion  orders  versus  time\n“, 
AUVORDERSFILENAME) ; 
fprintf  (auvordersfile, 

\n" ; ; 

fprintf  (auvordersfile, 
timestep:  %4.2f  secondsXn",  dt); 
fprintf  (auvordersfile, 

; 

fprintf  (auvordersfile. 


time  heading  North  East  Depth 
lateral  \n“ ) ; 

rpm 

rpm 

stern 

stern 

vertical 

fprintf  (auvordersfile, 

“#  x  y  z 

port 

stbd 

plane 

rudder 

thrusters 

thrustersXn “ ) ; 

fprintf  (auvordersfile, 

bow/ stern 

bow/stern\n“ ) ; 

fprintf  (auvordersfile, 

•\n“ ) ; 

} 

else  if  (TRACE  &&  DISPLAYSCREEN) 

printf  (■ (auvscriptf ile  checks  out  as  ready ...] \n" ) ; 

read_another_line  =  TRUE; 
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while  {read_anocher_line  ==  TRUE)  /‘^  *********  parse  loop  ************  */ 
{ 

parameterl  =  0.0; 
parameteiv  =  0.0; 
r  araiTiet  er3  =  0.0; 

if  (KEYBOARDINPUT  ==  TRUE) 

{ 

strcpy  (buffer,  "Enter  mission  script  command"); 
send_data_on_virtual_world_socket  {);  /*  buffer  msg  sent  */ 
printf  ("\n%s  HERE  ",  buffer); 

gets  (command_buf fer ) ; 


strcpy  (command_buf fer,  " " ) ; 

fgets  (command^buf fer,  120,  auvscriptfile) ; 


if  (feof  (auvscriptfile)) 

{ 

if  (DIE PLAY SCREEN) 

printf  (“[EOF  condition:  (%s  copy)  mi ssion . script . backup,  file  closed] \n", 

AUVSCRIPTFILENAME) ; 

fclose  (auvscriptfile); 
auvscriptf ileguit  =  TRUE; 
read^another^line  =  FALSE; 
end_test  =  TRUE; 

strcpy  (coiTumand^buf  fer,  "  “ )  ; 
break ; 


if  ( (int) (strlen  (command_buf fer)  <=  120)  &&  TRACE  &&  DISPLAYSCREEN) 

printf  (“strlen  (command_buf f er }  =  %d",  strlen  (command_buf fer ) ) ; 
printf  (“>>>%s<«“,  command^buf  fer)  ; 


parameters^read  =  sscanf  (comiriand^buf fer,  *%s",  keyword); 

if  'TRACE  &&  DISPLAYSCREEN) 

{ 

printf  ( “parameters_read  =  %d,  keyword  =  %s“, 
parairieters_read,  keyword)  ; 


for  (index=0;  index<=  ( int )  strlen  (keyword);  indeX‘K+)  /*  set  uppercase  */ 
keyword  [index]  =  toupper  (keyword  [index]); 

audible^coirjTiand  =  TRUE; 

if  (TRACE  &&  DISPLAYSCREEN) 

{ 

printf  (",  uppercase  keyword  =  %s\n",  keyword); 


if  ( (parameters_read  !=  1)  II 

(strlen  (keyword)  ==  0)  ii 

(strlen  (command_buf f er )  ==  0)  II 

{command_buffer  10]  ==  '\n'))  /*  blank  line  */ 

{ 

audible_command  =  FALSE; 
printf  (•\n"); 

} 

else  if  (  keyword  [0]  ==  '#')  /*  comment  */ 

{ 

if  (DISPLAYSCREEN)  printf  ("%s",  coTnmand__buffer)  ; 
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comrriand_buf f er  [0] 

) 

else  if  {(  (keyword  [0] 

(  ( keyword  [ 0 ] 

if  (DISPLAYSCREEN)  printf  ("%s“,  coinmand_buf f er )  ; 
comjnand_buf f er  (0]  =  '  '; 

coinmand^buf  f er  [1]  =  ' 

else  if  ( (scrcmp  (keyword,  “HELP")  ==  0)  il 
(strcmp  (keyword,  "?")  ==  0)  II 

(sLrcinp  (keyword,  ==  0)  II 

(strcmp  (keyword,  "/?“)  ==  0)) 

{ 

if  (DISPLAYSCREEN)  printf  (“[HELP]  “); 
print_valid_keywords  ( ) ; 

else  if  ((strcmp  (keyword,  “WAIT")  ==  0)  II 

(strcmp  (keyword,  “RUN")  ==  0)) 

■ 

parameters  read  =  sscanf  (command^buf f er ,  “%s%lf", 

^  "  keyword,  &  parameterl); 

print f  ('‘[%s  %6.2f;  ",  keyword,  parameterl); 

if  {  (parameters_read  ==  2)  (parameterl  >=  0.0)) 

{ 

read_another_line  =  FALSE; 
t ime_next_command  =  t  +  parameterl; 
printf  ("tiiTie  of  next  command  %6.2f]\n", 
time_next_command} ; 
fprintf  (auvordersf ile. 

If  %5.1f  %5.if  %5.1f  %6.1f  %6.1f  %6.1f  %6.1f  %5.1f  %5.1f  %5.1f 

f  «i 

t,  psi^command,  x_command,  y^command,  z_command, 
port_rpm_command,  stbd_rpm_command , 
rudder_command ,  planes_command, 
bow_lateral_thruster_comrriand, 
stern_lateral_thruster_command, 
bow_vert  ical_thruster_command, 
stern_vertical_thruster_command) ; 

else  printf  {"  warning:  illegal  time  value,  ignoredXn" ) ; 

else  if  ((strcmp  (keyword,  "TIME")  ==  0)  M 

(strcmp  (keyword,  "WAITUNTIL")  ==  0)  M 
(strcmp  (keyword,  “PAUSEUNTIL")  ==  0)) 

parameters^read  =  sscanf  (command^buf fer ,  “%s%lf"# 

keyword,  &  parameterl); 

printf  ("[%s  %6.2f]\n",  keyword,  parameterl); 

if  (parameters_read  ==  2) 

{ 

read_another_line  =  FALSE; 
time_next_command  =  parameterl; 
fprintf  (auvordersf ile, 

If  %6.1f  %5.1f  %5.1f  %5.1f  %6,lf  %6.1f  %6.1f  %6.1f  %5.1f  %5,lf  %5.1f 

^  '  t,  psi_command,  x_command,  y_co3Timand,  z^command, 

po  r t_r pm_c  omma  nd ,  s  t  bd_rpm_c  ommand , 
rudder_command,  pi an es_c ommand, 
bow_l  a  t  e  r  a  1_  th  ru  s  t  e  r_c  omma  nd , 
stern_lateral_thruster_command, 
bow_vert i ca l_thrus  ter_command , 
stern_vertical_thruster_command) ; 
if  (parameterl  <=  t) 

{ 

t  =  parameterl; 

printf  (“  warning:  time  value  has  reset  AUV  clock. \n"}; 


==  '/')  &&C  (keyword  [1]  ==  '/'))  II 
=  =  '/S  &C&  (keyword  [1]  ==  '*•)))  /*  comment  »/ 
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read_another_l ine  =  TRUE;  /*  no  PDU  */ 

} 

} 

else  printf  ( "  warning:  illegal  tiine  value,  ignored .  Xn" )  ; 

1 

else  if  ( (strcmp  (keyword,  "TIMESTEP")  ==0}  II 
{strcmp  (keyword,  “TIME-STEP*)  ==  0)} 

{ 

if  (sscanf  ( comma nd_buffer,  ■%s%F“,  keyword,  &parameterl)  ==  2) 

{ 

if  { (parameterl  >  0.0)  &&  (parameterl  <=  5.0)) 

{ 

dt  =  parameterl; 
if  (DISPLAYSCREEN) 

printf  (“[TIMESTEP  %6.2f]  “,  dt) ; 
fprintf  (auvordersf ile,  timestep:  %4,2f  secondsXn",  dt); 

1 

else  print_help  =  TRUE; 

} 

else  print_help  =  TRUE; 

} 

else  if  ((strcmp  (keyword,  “PAUSE*)  ==  0)  II 
(strcmp  (keyword,  "-PAUSE*)  ==  0)) 

{ 

if  (DISPLAYSCREEN) 
i 

printf  ( “ [PAUSE] \n* ) ; 

strcpy  (buffer,  “  Press  any  key  to  continue"); 
send_data_on_virtual_world__socket  ();  I*  buffer  msg  sent  */ 
printf  ("\n%s  ***  HERE  ♦**:  “,  buffer); 

answer  =  getchar  ();  /*  pause  *J 


} 

else  if  ((strcmp  (keyword,  “KEYBOARD")  ==  0)  II 
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{strcinp  (keyword,  “ KEYBOARD- ON “  }  ==  0)  M 

(strcmp  (keyword,  “KEYBOARD- INPUT" )  ==  0)  II 

(strcinp  (keyword,  “KEYBOARDINPUT" )  ==  0)) 

/ 

if  (DISPLAYSCREEN)  print f  (“(%s]\n“,  keyword); 

KEYBOARDINPUT  =  TRUE; 

) 

else  if  {  (strcinp  (keyword,  “KEYBOARD-OFF" )  ==  0)  II 

(strcmp  (keyword,  “ NO- KEYBOARD “ )  ==  0)) 

{ 

if  (DISPLAYSCREEN)  printf  (“[%s]\n“,  keyword); 

KEYBOARDINPUT  =  FALSE; 


else  if  ( (strcmp 

(keyword. 

“NOWAIT") 

== 

0) 

(strcmp 

(keyword, 

“NO-WAIT“ ) 

== 

0) 

(strcmp 

(keyword. 

“NOREALTIME") 

== 

0) 

(strcmp 

(keyword. 

“NO-REALTIME- ) 

=  =r 

0) 

(strcmp 

(keyword, 

“NONREALTIME" ) 

=  = 

0) 

(strcmp 

(keyword. 

“NO- PAUSE") 

=  = 

0) 

(strcmp 

(keyword. 

“NO PAUSE" ) 

=  = 

0) 

if  (DISPLAYSCREEN)  printf  (“[%s]\n“,  keyword); 
REALTIME  =  FALSE; 


(strcmp 

(keyword. 

“QUIT" ) 

=  = 

0) 

i  1 

(strcmp 

(keyword. 

“STOP") 

== 

0) 

1  ! 

(strcmp 

(keyword. 

“DONE" ) 

0) 

1 1 

(strcmp 

(keyword, 

“EXIT") 

=  = 

0) 

1  1 

(strcmp 

(keyword. 

“REPEAT" ) 

=  = 

0) 

1  1 

(Strcmp 

(keyword, 

“RESTART" ) 

== 

0) 

1 1 

(strcmp 

(keyword, 

“COMPLETE") 

0) 

1 1 

(strcmp 

(keyword , 

“KILL") 

=  s: 

0) 

1 1 

(strcmp 

(keyword, 

“ SHUTDOWN " ) 

=  = 

0) : 

( 

/*  note  this  command  does  not  reset  LOOPFOREVER,  except  for  */ 
/*  KILL/SHUTDOWN  which  terminate  the  dynamics  model  connection  */ 
if  '(Strcmp  (keyword,  “KILL")  ==  0)  II 

(strcmp  (keyword,  “SHUTDOWN")  ==  0)) 

{ 

LOOPFOREVER  =  FALSE; 
strcpy  (buffer,  “KILL"); 

send_data_on_virtual_world_socket  ();  /*  buffer  msg  sent  */ 

) 

printf  (“(%s]\n",  keyword); 

if  (DISPLAYSCREEN)  printf  {“(end_test  set  TRUE]\n“); 
end_test  =  TRUE; 

read_another_line  =  FALSE; 


fclose  (auvscriptf ile) ; 
auvscriptf ilequit  =  TRUE; 
if  (DISPLAYSCREEN) 

print f (“ (QUIT  condition:  (%s  backup  file)  mission . script . backup,  file  closed]\n“, 

AUVSCRIPTFILENAME) ; 

fprintf  (auvordersf ile, 

“%6.1f  %6.1f  %5.1f  %5.1f  %5.1f  %6.1f  %6.1f  %6.1f  %6.1f  %5.1f  %5.1f  %5.1f 

%5.1f \n“, 

t,  psi_coinmand,  x^command,  y_command,  z^command, 
po r t_rpm_c ommand ,  s t bd_rpm_c oinmand , 
rudder^command,  pi anes_c ommand, 
bow_lateral_thruster_command, 
stern_lateral_thruster_command, 
bow_vertical_thruster_command, 
stern_vertical_thruster_command) ; 
x_dot  =  0.0; 

y_dot  =  0.0; 

z_dot  =  0.0; 
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phi^dot 

= 

0.0; 

/*  degrees/ sec 

*/ 

theta_dot 

0.0; 

/*  degrees/sec 

*/ 

psi_dot 

= 

0.0; 

/*  degrees/sec 

V 

speed 

= 

0.0; 

u 

= 

0.0; 

V 

0.0; 

w 

= 

0.0; 

p 

= 

0.0; 

/*  degrees/sec 

*/ 

q 

= 

0.0; 

/*  degrees/sec 

V 

r 

= 

0.0; 

/*  degrees/sec 

*/ 

del ta_pl anes 

= 

0.0; 

/*  degrees 

*! 

delta_rudder 

= 

0.0; 

/*  degrees 

*  / 

porter pm 

0; 

stbd_rpm 

= 

0; 

vert ical_thruster_vol ts 

0.0; 

lateral_thruster_volts 

= 

0.0; 

if  ( (strcmp 

(keyword, 

•RPM“ ) 

=  =  0) 

( s  t  r  ciTip 

(keyword, 

-SPEED" ) 

=  =  0) 

i s  t  r  cmp 

(keyword. 

"PROPS" ) 

=  =  0) 

( s  t  r  cmp 

(keyword, 

"PROPELLORS") 

=  =  0)) 

parameters_read  =  sscanf  (command^buf fer, 

keyword,  &  parai^eterl 
fit  paraineter2 )  ; 

if  (parameters^read  ==  3) 


princf  {•‘[%s  %6.2f  %6.2f]\n-, 

port_rpin_cominand  =r  parameter!; 
stbd_rpm_coiTUTiand  =  parameter2; 


keyword, 
pararneter2 ) 


parameter! , 


parameters_read  =  sscanf  (command^buf  fer ,  ''%s%!f", 

.  keyword,  &  parameter!); 

printf  fM%s  %6.2f]\n-,  keyword,  parameter!); 

if  (parameters_read  ==  2) 

{ 

port_rpm._command  =  parameter!; 
stbd_rpm_command  =  parameter!; 

else  printf  ( “  warning:  no  value,  ignoredXn"); 


else  if  ( (strcmp  (keyword,  "COURSE")  ==  0)  || 

(strcmp  (keyword,  "HEADING")  ==  0)  || 

(strcmp  (keyword,  "YAW")  ==  0)) 

parameters_read  =  sscanf  (command_buffer ,  "%s%lf", 

pri„«  (-1%=  keyword,  ‘ 

if  {paraiiieters_read  ==  2) 

( 

DEADSTICKRUDDER  =  FALSE; 

WAYPOINTCONTROL  =  FALSE; 
psi_coinniand  =  parameter!; 

rotate_coiniiiand  =  0.0; 
lateral_command  =  0.0; 

RC5TATECONTROL  =  FALSE; 

LATERALCONTROL  =  FALSE; 

) 

^  else  printf  (•  warning:  no  value,  ignored\n‘); 

else  if  ((strcmp  (keyword,  ■TURN")  ==  0)  || 

(strcmp  (keyword,  *CHANGE-COURSE' )  ==  0)) 
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} 

else 

{ 


else 


) 

else 

{ 


} 

else 


parameters_read  =  sscanf  (cominand_buf f er,  •%s%lf"/ 

keyword/  &  parameter!); 

printf  {*'[%s  %6.2f)\n“,  keyword,  parameter!); 

if  (parameters_r6ad  ==  2) 

{ 

DEADSTICKRUDDER  =  FALSE; 

WAYPOINTCONTROL  =  FALSE; 

ROTATECONTROL  =  FALSE; 

psi_command  =  psi^command  +  parameter! ; 

} 

else  printf  ( “  warning:  no  value,  ignoredXn" ) ; 

if  (strcmp  {keyword,  "RUDDER**)  ==  0) 

parameters_read  =  sscanf  {command_buf f er ,  ■%s%lf", 

keyword,  &  parameter!); 

printf  ("[%s  %6.2f]\n",  keyword,  parameter!); 

if  (parameters_read  ==  2) 

{ 

DEADSTICKRUDDER  =  TRUE; 

WAYPOINTCONTROL  =  FALSE; 

ROTATECONTROL  =  FALSE; 

HOVERCONTROL  =  FALSE ; 
rudder_coinmand  =  parameter!; 

} 

el  se 
i 

printf  ( "  warning:  improper  value,  rudder  order  ignoredXn"); 


if  (strcmp  (keyword,  "DEADSTICKRUDDER")  ==  0) 


pa:'ameters_read  =  sscanf  (  command_buf  f  er , 
if  ;,parameters_read  ==  2) 


-%s%lf " , 
keyword, 


&  parameter!); 


printf  (•[%s  %6.2f]\n*‘,  keyword,  parameter!); 

DEADSTICKRUDDER  =  TRUE; 

WAYPOINTCONTROL  =  FALSE; 

ROTATECONTROL  =  FALSE; 

HOVERCONTROL  =  FALSE; 
rudder_command  =  parameter!; 


printf  ("l%s]  ",  keyword); 

DEADSTICKRUDDER  =  TRUE; 

WAYPOINTCONTROL  =  FALSE; 

ROTATECONTROL  =  FALSE; 
rudder^command  =  0.0; 

printf  {"  warning:  improper  value,  rudder  set  to  0\n"); 


if  (strcmp  (keyword,  "DEPTH")  ==  0) 

parameters^read  =  sscanf  (command_buf fer,  ■%s%lf", 

keyword,  &  parameter!); 

printf  {•I%s  %6.2f]\n",  keyword,  parameter!); 

if  (parameters_read  ==  2) 

{ 

DEADSTICK PLANES  =  FALSE; 
z_command  =  parameter!; 

} 

else  printf  ("  warning:  no  value,  ignoredXn"); 
if  (strcmp  (keyword,  "PLANES")  ==  0) 
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parameters_read  =:  sscanf  (coinmand^buf f er ,  -%s%lf", 

keyword,  &  parameter!) 

printf  {"(%s  %6.2f]\n'',  keyword,  parameterl )  ; 

If  (parameters_read  ==  2) 

I 

DEADSTICKPLANES  =  TRUE; 

^  planes_command  =  parameterl; 

else  printf  C  warning:  improper  value,  planes  order  ignoredXn") 
if  {strcmp  (keyword,  "DEADSTICKPLANES * )  ==  0) 

parameters_read  =  sscanf  (command^buf f er,  "%s%lf", 

..  ,  keyword,  &  parameterl) 

if  (parameters^read  ==2) 

{ 

printf  %6.2f]\n“,  keyword,  parameterl); 

DEADSTICKPLANES  =  TRUE; 
planes_command  =  parameterl; 


printf  {•[%s]  keyword); 

DEADSTICKPLA.NES  =  TRUE; 
planes^comiTiand  =  0.0; 

printf  (“  warning:  improper  value,  planes  set  to  0\n"); 


if  ((strcmp 

(keyword. 

"THRUSTERS-ON") 

=  =  0) 

( s  t  r  cmp 

(keyword, 

•THRUSTERS’ ) 

=  =  0) 

(strcmp 

(keyword. 

•THRUSTERON" ) 

=  =  0) 

(strcmp 

(keyword. 

■THRUSTERSON" ) 

==  0)) 

printf  (M^s 
THRUSTERCONT 

)\n“,  keyword); 

ROL  =  TRUE; 

if  n strcmp 

(keyword. 

•NOTHRUSTER’ } 

==  0) 

(strcmp 

(keyword, 

•NOTHRUSTERS" ) 

==  0) 

(strcmp 

(keyword. 

’THRUSTERS-OFF’ ; 

1  ==  0) 

(strcmp 

(keyword. 

’THRUSTERSOFF" j 

==  0) 

printf  ("[%sj\n",  keyword); 

THRUSTERCONTROL  =  FALSE; 

if  (strcmp  (keyword,  "ROTATE")  ==  0) 

parameters_read  =  sscanf  ( command_buf f er ,  •%s%lf'', 

-  keyword,  &  parameterl! 

pi  ^nti  (  i%s  %6.2f]\n*',  keyword,  parameterl )  ; 
if  (parameters_read  ==  2) 

1 

THRUSTERCONTROL  =  TRUE; 

WAYPOINTCONTROL  =  FALSE; 

HOVERCONTROL  =  FALSE; 
rotate_command  =  parameterl; 

clamp  (trotate  command,  -12.0,  12.0,  Tocate  command*); 
lateral_command  =0.0;  ~ 

ROTATECONTROL  =  TRUE; 

LATERALCONTROL  =  FALSE; 

) 

else  printf  (•  warning:  no  value,  ignored\n"); 

if  ((strcmp  (keyword,  'NOROTATE")  ==  0)  1| 

(strcmp  (keyword,  •ROTATEOFF")  ==  0)  || 

(strcmp  (keyword,  "ROTATE-OFF")  ==  O)) 
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printf  ('‘[%s]\n*',  keyword); 
rotate_coiTunand  =  0.0; 

ROTATECONTRCL  =  FALSE; 

} 

else  if  (strcmp  (keyword,  "LATERAL")  ==  0) 

{ 

parameters_read  =  sscanf  (command_buf fer , 

keyword,  &  parameterl ) ; 

printf  (•l%s  %6.2f]\n",  keyword,  parameterl ) ; 

if  (parameters_read  ==  2) 

{ 

THRUSTERCONTROL  =  TRUE; 

WAYPGINTCONTROL  =  FALSE; 

HOVERCONTROL  =  FALSE; 

rctate^command  =  0.0; 
lateral_coininand  =  paraiteterl; 

clamp  (&  lateral^command,  -0.5,  0.5,  "lateral^command" ) ; 
ROTATECONTROL  =  FALSE; 

LATERALCONTROL  =  TRUE; 

) 

else  printf  ("  warning:  no  value,  ignoredXn" ) ; 

) 

else  if  ( (strcmp  (keyword,  “NOLATERAL")  ==  0)  II 

'istrcmp  (keyword,  “LATERALOFF" )  ==  0)  II 

(Strcmp  (keyword,  “ LATERAL- OFF " )  ==  0)) 

printf  ( “ L%s] xn" ,  ke>^ord) ; 
la  teral^coiTLTiand  =  0.0; 

LATERALCONTROL  =  FALSE; 


else  if  {(strcmp  (keyword,  “POSITION")  ==  0)  II 

(Strcmp  (keyword,  “LOCATION")  ==  0)  ) 


pa 

pr 

if 


ameters^read  =  sscanf  (commiand^buf  f  er ,  “%s%lf  %I  f %lf  " , 

keyword,  &  parameterl, 

&  parameter2,  &  parameters); 

ntf  (‘'(%s  %6.2f  %6.2f  %6.2f]\n",  keyword,  parameterl, 

parameter2,  parameters); 


(pararrieters_read  ==  A] 


X  =  parameterl; 

y  =  parameters,- 

z  =  parameters,' 

skip  line  in  telemetry  file  to  break  point-to-point  lines  */ 
fprintf  (auvdataf ile,  "\n“); 

} 

else  printf  {"  warning:  invalid  x/y/z  position,  ignored\n“); 

) 

else  if  ((Strcmp  (keyword,  “ORIENTATION")  ==  0)  II 

(strcmp  (keyword,  "ROTATION")  ==  0)  ) 

parameters_read  =  sscanf  (command_buf fer ,  •%s%lf %lf%lf ■ , 

keyword,  &  parameterl, 

&  parameters,  &  parameters); 

printf  ("[%s  %6.2f  %6.2f  %6.2f]\n",  keyword,  parameterl, 

parameter 2 ,  parameters ) ; 

if  (parameter spread  ==  4) 

{ 

phi  =  parameterl; 
theta  =  parameters,* 
psi  =  parameters,* 

} 

else 

printf  ("  warning:  invalid  phi/theta/psi  orientation,  ignoredXn"); 

else  if  ((strcmp  (keyword,  "CONTINUE")  ==  0)  M 
(strcmp  (keyword,  “GO")  ==  0)) 
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if  (DISPLAYSCREEN)  print f  (•(%s]*,  keyword); 
return;  /*  no  action  required  */ 

if  ( (strcmp  (keyword,  "STEP")  ==  0)  I 

(strcmp  (keyword,  ’SINGLE-STEP")  ==  0)) 

if  (DISPLAYSCREEN)  printf  ("[%s]",  keyword); 
tiine_next_coirariand  =  t  *  dt  ; 
read_another_line  =  FALSE; 

if  ((strcmp  (keyword,  ’TRACE")  ==  0)  || 

(strcmp  (keyword,  "TRACE-ON")  ==  0)) 

if  (DISPLAYSCREEN)  printf  ("[TRACE  =  TRUE]  ’) 
TRACE  =  TRUE; 


(Strcmp 

(keyword. 

’TRACEOFF’) 

==  0) 

(strcmp 

(keyword, 

’TRACE- OFF" ) 

==  0) 

(strcmp 

(keyword. 

"NOTRACE" ) 

==  0) 

(strcmp 

(keyword, 

"NO-TRACE" ) 

==  0) 

if  (DISPLAYSCREEN)  printf  ("[TRACE  =  FALSE]  ’); 

TRACE  =  FALSE; 

if  nstrcmp  (keyword,  "LOOPFOREV'ER* )  ==  0)  I! 

(s-rcmp  (keyword,  "LOOP-FOREVER" )  ==  0)) 

if  (DISPLAYSCREEN)  printf  ( " [LOOPFOREVER ]  "); 
LOOPFOREVER  =  TRUE; 

if  ((strcmp  (keyword,  "LOOPONCE")  ==  0)  || 

(strcmp  (keyword,  "LOOP-ONCE")  ==  0)) 

if  (DISPLAYSCREEN)  printf  (’[LOOPONCE]  "); 

LOOPFOREVER  =  FALSE; 

if  {(Strcmp  (keyword,  ’LOOPFILEBACKUP" )  ==  0)  II 
(strcmp  (keyword,  ’LOOP-FILE-BACKUP")  ==  O) ) 

if  (DISPLAYSCREEN)  printf  ("[LOOPFILEBACKUP]  "); 
LOOPFILEBACKUP  =  TRUE; 

if  ((Strcmp  (keyword,  " ENTERCONTROLCONSTANTS " )  ==  0)  || 

(strcmp  (keyword,  " ENTER- CONTROL- CONSTANTS " )  ==  0)) 

if  (DISPLAYSCREEN)  printf  (’[ENTERCONTROLCONSTANTS]  ’)• 
ENTERCONTROLCONSTANTS  =  TRUE;  ’ 

get_control_constants  (); 
fflush  (stdin); 

if  ((strcmp  (keyword,  ’SLIDINGMODECOURSE’ )  ==  O)  II 

(Strcmp  (keyword,  ‘ SLIDING-MODE-COURSE " )  ==  0)) 

printf  (’(%s  =  TRUE]\n’,  keyword); 

SLIDINGMODECOURSE  =  TRUE; 

WAYPOINTCONTROL  =  FALSE; 

ROTATECONTROL  =  FALSE; 

HOVERCONTROL  =  FALSE; 

if  ((strcmp  (keyword,  ’SLIDINGMODEOFF’ )  ==  0)  II 

(strcmp  (keyword,  ’SLIDING-MODE-OFF’)  ==  O) ) 

printf  (’[%s:  SLIDINGMODECOURSE  =  FALSE] \n’,  keyword)- 
SLIDINGMODECOURSE  =  FALSE; 


if  ((strcmp  (keyword,  ’TACTICAL’; 


==  0)  II 
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(strcmp  {keyword,  "TACTICAL-ON" ) 


==  0)) 

prlntf  ("[%s]\n'‘,  keyword); 

TACTICAL  =  TRUE; 

} 

else  if  (  (strciT^p  (keyword,  “NO-TACTICAL")  ==  0)  I  I 
(strcmp  (keyword,  " TACTICAL- OFF " )  ==  0)) 

{ 

printf  ("(%s]\n",  keyword); 

TACTICAL  =  FALSE; 

} 

else  if  (strcmp  (keyword,  "SONARTRACE" )  ==  0) 

{ 

if  (DISPLAYSCREEN)  printf  ("[SONARTRACE]  "); 
SONARTRACE  =  TRUE; 

else  if  (strcmp  (keyword,  " SONAR TRAC EOFF " )  ==  0} 

t 

if  (DISPLAYSCREEN)  printf  ( " [ SONARTRACEOFF ]  "); 
SONARTRACE  =  FALSE; 

else  if  (strcmp  (keyword,  " SONAR INSTALLED " )  ==  0) 

if  (DISPLAYSCREEN)  printf  (" [SONAR INSTALLED]  "); 
SONAR INSTALLED  =  TRUE; 

} 

else  if  (strcmp  (keyword,  " PARALLELPORTTRACE " )  ==  0) 

if  (DISPLAYSCREEN)  printf  {"[PARALLELPORTTRACE]  "); 
PAPA.LLELPORTTRACE  =  TRUE; 


(strcmp 

(keyword. 

"AUDIBLE*  ) 

==  0)  1 

(strcmp 

(keyword. 

"AUDIO* ) 

=  =  0)  1 

(Strcmp 

(keyword. 

"AUDIO- ON") 

==  0;  1 

(strcmp 

(keyword. 

"SOUND- ON") 

=  =  0)  1 

(strcmp 

(keyword. 

"SOUNDON" ) 

==  0}  i 

(Strcmp 

(keyword. 

"SOUND" ) 

=  =  0)) 

if  (DISPLAYSCREEN;  printf  (" [AUDIBLE]  "); 

strcpy  (buffer,  "AUDIBLE");  /*  copy  current  command  to  buffer  */ 
serid_data_on_virtual_world_socket  {);  J*  send  to  sound  driver  */ 


else  if  ((strcmp  (keyword,  "SILENT")  ==  0) 

(strcmp  (keyword,  "SILENCE")  ==  0) 

(strcmp  (keyword,  "NOSOUND" )  ==  0) 

(strcmp  (keyword,  "SOUNDOFF")  ==  0) 

(strcmp  (keyword,  "SOUND-OFF")  ==  0) 

(strcmp  (keyword,  "AUDIOOFF")  ==  0) 

(strcmp  (keyword,  "AUDIO-OFF")  ==  0) 
(strcmp  (keyword,  "QUIET")  ==  0)) 

{ 


if  (DISPLAYSCREEN)  printf  ("[SILENT]  "); 

strcpy  (buffer,  "SILENT");  /*  copy  current  command  to  buffer  */ 
send_data_on_virtual_world_socket  ();  /*  send  to  sound  driver  */ 


else  if  ((strcmp 

(keyword,  ’EMAIL*) 

==  0) 

(strcmp 

(keyword,  ’EMAIL-ON*) 

==  0) 

(strcmp 

(keyword,  ’E-MAIL*) 

==  0) 

(strcmp 

(keyword,  ’E-MAIL-ON*) 

==  0) 

(strcmp 

(keyword,  *EMAILON*) 

==  0) 

if  (TRACE  &&  DISPLAYSCREEN)  printf  ("[EMAIL  ON]  •); 
EMAIL  =  TRUE; 

) 

else  if  ((strcmp  (keyword,  "EMAILOFF")  ==  0)  II 
(strcmp  (keyword,  "EMAIL-OFF")  ==0)  II 
(strcmp  (keyword,  "E-MAILOFF")  ==  0)  I  I 


else  if 


(strcmp 

(keyword, 

“E-MAIL-OFF* ) 

==  0)  11 

(strcmp 

(keyword , 

“NO-E-MAIL" ) 

==  0)  11 

(strcmp 

(keyword , 

“NO-EMAIL" ) 

==  0)  11 

(strcmp 

(keyword. 

“NO-E-MAIL“) 

==  0)  II 

(strcmp 

(keyword. 

“NOEMAIL") 

=  =  0)) 

if  (TRACE 
EMAIL  =  FA 

&&  DISPLAYSCREEN)  printf 
LSE; 

(“ [EMAIL 

if  ( (strcmp 

(keyword. 

•WAYPOINT" ) 

==  0)  II 

(strcmp 

(keyword, 

“ WAYPOINT- ON “) 

==  0)) 

parameters^read  =  sscanf  (conirnand_buf fer ,  ■%s%If %lf %If • , 

keyword,  &  parameterl, 

&  paraineter2,  &  parameters}, 

if  (parameters_read  ==  4) 

{ 

printf  ("[^s  %6.2f  %6.2f  %6.2f]\n*’,  keyword,  parameter!, 

parameter2,  parameters); 

WAYPCINTCONTROL  =  TRUE; 

x_command  =  parameter!; 

y_command  =  parameters; 

2_command  =  parameters; 

port_rpm_command  =  fabs  (port_rpm_command} ;  /*  ensure  fwd  */ 

stbd_rpm:_commiand  =  fabs  {stbd_rpm_command)  ;  /*  motion  only  */ 

fprint f  (auvordersf ile, 

•If  %c.lf  %5.!f  %5.!f  %5.1f  %6.1f  %6.!f  %6.!f  %6.!f  %5-lf  %5.!f  %5-!f 

■  £  ^  i. 

t,  psi_command,  x_command,  y_comjnand,  z_command, 
p  o  r  t _rpm_c  omma  nd ,  s  t  bd_r pm_c  omma  nd , 
rudder_command,  planes_command, 
bow_lateral_thruster_command, 
s  t  e  rn_l a  t e  ra l_t h  ru  s  t  er_c  omma  nd , 
bow_vertical_thruster_command, 
stern_verti cal_chruster_command ) ; 

) 

else  if  (parameters_read  ==  3) 

printf  {'‘[%s  %6.2f  %6.2f]\n*',  keyword,  parameter!, 

parameters ) ; 

WAYPOINTCONTROL  =  TRUE; 

x_command  =  parameter!; 

y_command  =  parameters,* 

port_rpm_coirjnand  =  fabs  (port^rpm^command)  ;  /*  ensure  fwd  */ 

stbd_rpm_coinmand  =  fabs  (stbd_rpm_command)  ;  /*  motion  only  */ 

fprintf  (auvordersf ile, 

.jf  %6.!f  %5.1f  %5.!f  %5.!f  %6.!f  %6.!f  %6.1f  %6.!f  %5.!f  %5.!f  %5.1f 


t,  psi^command,  x_command,  y^command,  z_command, 
port_rpm_command ,  stbd_rpm_command , 
rudder_command,  planes_command, 
bow_l a  tera l_thruster_command , 
stern_lateral_thruster_command, 
bow_vertical_thruster_coinmand, 
stern_vertical_thruster_command}  ; 

) 

else 

{ 

WAYPOINTCONTROL  =  FALSE; 

printf  (^n  warning:  improper  number  of  valuesXn  waypoint*); 
printf  {“set  to  current  position  but  otherwise  ignored\n“); 

x_command  =  x; 

y^command  =  y; 

2_command  =  z; 


fprintf  (auvordersf ile, 

•%6.1f  %6.!f  %5.1f  %5.!f  %5.1f  %6.!f  %6.!f  %6.1f  %6.1f  %5.1f  %5  if  if 

%5.!f\n“. 
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psi_comiTiand,  x_command,  y_cominand,  z_conunand, 

P o t _r piTuc omma n d ,  s t bd_r pin_c omin and, 
rudder_command,  planes^command, 
bow_l  a  tera  l_chrust€r_cominand , 
s  t  e  r  n_l  a  t  e  r  a  t  h  ru  s  t:  er_c  omma  nd , 
bow_vertical_thruster_command, 
s  t  e  r n_v e  r  t  i  c  a  I_t hiru  s  t  er_c  omma  nd )  ; 

} 

if  (FOLLOWWAYPOINTMODE  ==  TRUE) 

{ 

if  (TRACE)  printf  C [FOLLOWWAYPOINTMODE  check] ") ; 

/*  continue  until  WAYPOINT  reached  without  further  script  orders  */ 
tim€_next_command  =  t-»-2.0*dt; 
read_arjother_line  =  FALSE; 

} 

} 

else  if  ( (strcmp  (keyword,  "WAYPOINTFOLLOW- )  ==  0)  II 

^  (strcmp  (keyword,  "WAYPOINT-FOLLOW )  ==  0)  ) 

printf  (“[%s]\n-,  keyword); 

FOLLOWWAYPOINTMODE  =  TRUE; 

) 

else  if  1 f strcmp  (keyword,  “WAYPOINTFOLLOWOFF" )  ==  0)  II 


) 

else 


(strcmp  (keyword,  “WAYPOINT-FOLLOW-OFF- )  ==  0)  ) 

printf  ("(^sjXn",  keyword); 

FOLLOWWAYPOINTMODE  =  FALSE; 


if  f (strcmp 

(keyword,  "STANDOFF" ) 

==  0) 

( s  t  rcmip-' 

(keyword,  "STAND-OFF" ) 

==  0} 

{strcmp 

(keyword,  "STANDOFFDI STANCE" ) 

==  0) 

( s  t  r  cmp 

(keyword,  " STANDOFF- DI STANCE " ) 

==  0) 

(strcmp 

(keyword,  "STAND-OFF-DISTANCE" 

)  ==  0)) 

parameters. 

_read  =  sscanf  (command^buf fer 

,  "%s%lf 
keyword, 

&  parameterl), 


printf  {-[%s  %6.2f]\n“,  keyword,  parameterl); 

standcf f_distanc6  =  parameterl; 


} 

else 

( 


else 

( 

printf  (•‘i'%s]\n-,  keyword); 

printf  [ "  warning:  no  standoff  value  provided,  ignored"); 


if  {(strcmp  (keyword,  "HOVER")  ==  0)  (| 

(strcmp  (keyword,  "HOVER-ON")  ==  0)) 

parameters_read  =  sscanf  (command_buf f er,  ■%s%lf %lf %lf %l f %if ■ , 

keyword,  &  parameterl, 

&  parameter2,  &  parameters, 

&  parameter4,  &  parameters); 

(parameters_read  ==  6) 


if 

{ 


printf  (-(%s  %6.2f  %6.2f  %6.2f  %6.2f  %6.2f]\n", 

keyword,  parameterl, 
parameter2 ,  parameters , 

HOVERCONTROL  =  TRUE;  parameter4,  parameters); 

WAYPOINTCONTROL  =  FALSE; 

ROTATECONTROL  =  FALSE; 

THRUSTERCONTROL  =  TRUE; 

DEADSTICKRUDDER  =  TRUE; 

FOLLOWWAYPOINTOODE  =  FALSE; 
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-%6.1f  % 

%S.lf \n-. 


rudder_comiriand  =  0.0; 
x_command  =  parameter!; 

y_command  =  parameter2; 

2_coininand  =  parameters; 

psi_comrr,and  =  parameter4; 

psi_command_hover  =  parameter4; 
standof f_distance  =  parameters; 
fprintf  (auvordersfile, 

6. If  %5.1f  %5.!f  %5.1f  %6.1f  %6.1f  %6.1f  %6.1f  %5.1f  %5.1f  %5.1f 

psi_command,  x_command,  y_command,  2_command, 
port_rpm_command,  stbd_rpm_command, 
rudder_command,  planes_coinmand, 
bow_l  a  t  e  r  a  l_t  h  ru  s  t  e  r_comma  nd , 
stern_lateral_thruster_command, 
bow_vertical_thruster_command, 
stern_vertical  thruster  command); 


else  if  {parameters_read  ==  5) 
{ 

printf  (-  [%s  %6.2f  %6.2f 


HOVERCONTROL  =  TRUE; 

WAYPGINTCONTROU  =  FALSE; 

RCTATECONTROL  =  FALSE; 

THRUSTERCONTRCL  =  TRUE; 

DEADSTICKRUDDER  =  TRUE; 

FOLLOWWAY POINTMODE  =  FALSE; 


%6.2f  %6.2f]\n-, 

keyword,  parameter! , 
parameter2,  parameters, 
parameter4 ) ; 


rudder^command 
x_c ommand 
y^comjnand 
z^commiand 
psi_c ommand 
psi_c ommand_hover 
fprintf  {auvordersf ile, 
%5.1f  %5.!f  %5.!f  %6.1f 


=  0.0; 

=  parameter!; 

=  parameter2; 

=  parameters; 

=  parameter4; 

=  parameter4; 

^6.!f  %6.1f  %6.!f  %5.If  %5.!f  %5.1f 


■%6.1f 
^5. If \n' 


%6.1f  %5. 


t,  psi_command,  x_command,  y_command,  z_command, 
port_rpm_c ommand ,  stbd_rpm_c ommand, 
rudder_commiand,  pi  anes_c  ommand, 

bow_l  a  teral_thruster_c  ommand, 
sternal  a  teraI_thruster_command, 
t>ow_vertical_thruster_command, 
stern_vertical_thruster_command} ; 

Ise  if  (parameters_read  ==  4) 

printf  %6.2f  %6.2f  %6.2f]\n*‘,  keyword,  parameter!, 

parameter2 ,  parameters ) ; 

HOVERCONTROL  =  TRUE; 

WAYPOINTCONTROL  =  FALSE; 

ROTATECONTROL  =  FALSE ; 

THRUSTERCONTROL  =  TRUE; 

DEADSTICKRUDDER  =  TRUE; 

FOLLOWWAYPOINTMODE  =  FALSE; 
rudder_command  =  0.0; 
x_command  =  parameter!; 

y_command  =  parameter2; 

z_command  =  parameters,* 

5rintf  (auvordersfile, 

,lf  %5.1f  %5.1f  %6.1f  %6.1f  %6.1f  %6.1f  %5.1f  %5.1f  %5.1f 

ps i__c ommand ,  x_command,  y^command,  z_^command, 
port_rpm_c ommand,  stbd_rpm_c ommand, 
rudder_command,  planes^command. 
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%6.if 
%  5 . i f \n " , 


) 

else 


bow_l  a  t  e  r  a  1  _t  h  ru  s  t  e  r_c  oirnna  nd , 
stern_lateral_thruster_coiTmtand, 
b  ow_v  e  r  t  i  c  a  l_t  h  ru  s  t  er_c  oinina  nd  ^ 
stern_vertical_thruster_coininand)  ; 

) 

else  if  (parameters_read  ==  3) 

{ 

printf  (■‘l%s  %6.2f  %6.2f]\n\  keyword,  parameter!, 

parameter 2 ) ; 

HOVERCONTROL  =  TRUE; 

WAYPOINTCONTROL  =  FALSE; 

ROTATECONTROL  =  FALSE; 

THRUSTERCONTROL  =  TRUE; 

DEADSTICKRUDDER  =  TRUE; 

FOLLOWWAYPOINTMODE  =  FALSE; 
rudder_command  =  0.0; 
x^command  =  parameter!; 

y_command  =  parameter2; 

fprintf  (auvordersf ile, 

%5.1f  %5.!f  %5.!f  %6.1f  %6.1f  %6.!f  %6.1f  %5.!f  %5.!f  %5.1f 

t,  psi_command,  x_command,  y_command,  2_command, 
port_rpm_command ,  stbd_rpm_command , 
rudder_command,  planes_command, 
bow_l a te r a l_th ru s t er_c ommand , 
stern_lateral_thruster_coinmand, 
bow_vertical_thruster_command, 
stern_vertical_thruster_command) ; 

} 

else  if  (parameters_read  ==  !) 

{ 

printf  (M^slXn",  keyword); 

HOVERCONTROL  =  TRUE; 

WAYPOINTCONTROL  =  FALSE; 

ROTATECONTROL  =  FALSE ; 

THRUSTERCONTROL  =  TRUE; 

DEADSTICKRUDDER  =  TRUE; 

FOLLOWWAYPOINTMODE  =  FALSE; 
rudder^command  =  0.0; 
fprintf  (auvordersf i le , 

%S.lf  %5.if  %S.lf  %6.1f  %6.!f  %6.1f  %6.!f  %5.!f  %5.1f  %5.!f 

t,  psi_command,  x_command,  y_command,  z_command, 
port_rpm_command ,  stbd_rpm_command , 
rudder_command ,  pi anes^command , 
bow_laterai_thruster_command, 
sternal a teral_thruster_command, 
b ow_v e r t i c a 1 _ t h r u s t e r _c omm a n d , 
stern_vertical_thruster_command) ; 

} 

else 

{ 

printf  {••[%s]\n*‘,  keyword); 

printf  ("  warning:  improper  number  of  values,  ignoredXn*); 

} 

parse_mission_string_commands  (command^buf fer) ; 

/*  check  other  possibilities  */ 


if  (audible^command  ==  TRUE) 

strcpy  (buffer,  command_buf fer ) ;  /*  copy  current  command  to  buffer  */ 
send_data_on_yirtual_world_socket  ();  /*  send  to  sound  driver  */ 

) 

if  {(print^help  ==  TRUE)  &&  DISPLAYSCREEN) 

{ 
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print f  command_buffer); 

print_vai jd_key words  ( ) ; 

strcpy  (buffer,  “  is  an  unknown  coinmand** }  ;  /*  copy  msg  to  buffer  */ 
send_da t a_on_vi rtual_worid_socket  ();  /*  send  to  sound  driver  */ 

print_help  =  FALSE;  /*  reset  value  */ 

}  /*  loop  until  read_another_line  is  FALSE)  */ 

if  {TRACE  DISPLAYSCREEN) 

printf  (**[end  parse_mission_script_coTrmiands  ()]\n"); 

return; 

/*  end  parse_niission_script_commands  ()  */ 


VO  JO  parse_iT;ission_string_comrriands  (command) 
cnar  *  command; 

int  index; 

int  number„values  =  0; 

char  parameter/  [60]; 

:f  'TFvACE  &&  TI SPLAYSCREEN) 

print:  ■:  "  \n  [parse^mi  ssron^st  ring_commands  start]  \n" )  ; 

nuT,Lei_values  =  sscanf  (command^buf fer,  *'%s-,  keyword); 

lor  (index  =  0;  index  <=  (int)  strlen  (keyword);  index^+) 
keyword  [index]  =  tcupper  (keyword  [index]); 

if  (num.ber_v3lues  *=  1) 

ir  (LISPLAYSCREENi  printf  ( “  [no  parse  word  found] Xn") 
return ; 


f  ( (strcmp 

( keyword. 

-REMOTEHOST" ) 

=  =  0) 

( strcmp 

( keyv;ord, 

"REMOTE" ) 

=  =  0) 

(strcmp 

(keyword, 

"REMOTE- HOST") 

1  ==  0) 

( St  rcmp 

( keyword, 

"HOST" ; 

=  =  0) 

if  (sscanf  (command,  ■%s  %s*',  keyword,  parameter2)  ==  2) 

( 

strcpy  ivirtual_worid_remote_host_name,  parameter2)  ; 
if  (DISPLAYSCREEN)  printf  C [REMOTE-HOST  %s]  “ , 

^  virtual_world_remote_host_name ) ; 

else  print_help  =  TRUE; 

) 

else  print_help  =  TRUE;  /*  invalid  command  line  entry  parameter  found  */ 
if  (TRACE  &&  DISPLAYSCREEN) 

printf  CXnlparse^mission^string^commands  complete] \n" ) ; 
return; 

}/*  end  parse_mission_string_coinmands  ()  */ 

/*********♦*♦********** ♦^ ************************** ***^*******^^^^^^^^^^^^^^^^^^ 
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n 

Program: 

Authors : 

Revised : 

Stem.: 
Compiler : 

Compilation : 

[68020] 

[68030] 

ilrix  j 


globals.c  Global  Variable  Instantiation 


globals.c 
Don  Brutzman 
21  October  94 


AUV  Gespac  68020/68030.  OS-9  version  2.4 
Gespac  cc  Kernighan  &  Richie  (K&R)  C 

ftp>  put  globals.c 
auvsimil>  chd  execution 
auvsiml>  make  “k2f  execution 
auvsj.ml>  make  execution 


flecch>  make  execution 

Allows  repeated  use  of  global  variables  global. c  via  global. h 
in  order  to  prevent  compiler  warnings 


/  ^  * 

*»»* »» »»»»★*»♦***★»»»***★*»* 

dude  “defines.h" 

»  * 

■»  * 

/*" 

/" 

Program  configuration  flags 

*  * 

★  ★ 

int 

TRACE 

0 

/* 

int 

DISPIAYSCREEN 

1 

/* 

I  n 

LOCATI ONLAB 

= 

1 

/* 

in: 

TACTICAL 

0 

/* 

1  n: 

LOC-PFOREVER 

= 

0 

/* 

in: 

LOC-PFILEBACKUP 

= 

1 

/* 

in: 

PArA.LLELPORTTRACE 

0 

/* 

in: 

SONAR INSTALLED 

0 

/* 

1  n: 

SONARTRACE 

— 

0 

/* 

in.. 

EN’TERCONTRCLCONSTANTS 

0 

/* 

in: 

REALTIME 

= 

1 

/* 

in: 

DEADRECKON 

0 

i* 

int 

DEADSTICKRUDDER 

0 

/* 

int 

DEADSTICKPLANES 

0 

/* 

int 

SLIDINGMODECOURSE 

= 

0 

/* 

int 

THRUSTERCONTROL 

0 

/* 

int 

ROTATECONTROL 

0 

/* 

int 

LATERALCONTROL 

- 

0 

/* 

int 

FOLLOWWAYPOINTMODE 

0 

/* 

int 

WAYPOINTCONTROL 

- 

0 

!* 

int 

#if 

HOVERCONTROL 

defined (sgi ) 

— 

0 

/* 

int  EMAIL 

#else 

1; 

/* 

int  EMAIL 

#endi  f 

0; 

/* 

int 

NOT_YET_RE IMPLEMENTED 

— 

0; 

/* 

f***-****ic**ititit*itii  , 


l=trace  on,  0=trace  off  */ 
l=screen  on,  0=screen  off  */ 
l=graphics  lab,  0=actual  vehicle  */ 
l=tactical  on,  0=tactical  off  */ 
l=repeat  execution  indefinitely  */ 
l=backup  files  between  replications*/ 

l=trace  each  char  received  at  port  */ 
l=sonar  head  available  for  query  */ 
l=trace  on,  0=trace  off  */ 
l=manual  entry,  0=default  values  */ 
1=1  second  real-time  waits,  O^none  */ 

l=dead  reckon  navigate,  0=regular  ♦/ 
l=use  ordered  rudder,  0  =  control  */ 
l=use  ordered  planes,  0  =  control  */ 
l=use  sliding  mode,  0  =  control  */ 

l=use  thrusters,  0=use  propellers  */ 
l=use  thrusters  to  rotate  in  place  */ 
l=use  thrusters  for  lateral  motion  */ 
1=  go  to  WAYPOINT  without  WAITS  */ 
1=  go  to  WAYPOINT  */ 
l=hover  at  WAYPOINT  */ 


1;  /*  Iscsend  e-mail,  0=don't  send  e-mail  */ 

0;  /*  can't  send  e-mail  via  OS-9  directly  */ 

0;  /*  code  in  block  needs  reverification  */ 
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dcutle-  TIME2TEP  =  0.1;  time  of  a  single  closed  loop  */ 

/’  add  code  to  warn  if  exceeded  ««  */ 

rnt  KEYBOAFX'INPUT  =  0;  /*  l=read  keyboard  vice  mission  file  */ 

■»>r»-rTr»'»''*-*»-*-»TrTr»*»--jr*-*-»r'*-*****'*ir****-*'*»*-r*Tr»'»*****-******»********'*********'*******/ 

files  and  paths  */ 

FILE  *  auvscriptf lie; 

FILE  *  auvcrdersf lie; 

FILE  *  auvdatafiie; 

FILE  auvt  ext  file; 

FILE  *  cent roiconstantsf i le; 

FILE  *  emailaddressf lie; 

/*  FILE  *  serialtestf ile;  */ 

int  serialpath  =  0; 

int  sonarpath  =  0; 

/»*•»»»»»■»•»*  +  »•»■*»■»»»»■*•*♦♦»»★»*■**★»»**»***■»»*»★■»★»*****»***■*■*♦★»★*♦*»*******»****/ 

’  Variables  and  data  structures  */ 

,*  buffers  of  full  strings  for  byte  transfer  to  tactical  level  &  disk  file  */ 

/’  'buffer'  usually  <  256,  intentionally  oversized  in  case  of  overflow  error  */ 

ti.Tie_t  sy  sterr.^time  =  0; 

struct  tm  *  sy  s  t ei7:_t mp  =  0  ; 

/*  partial  structure  template  for  the  MFI  (only  interested  in  PIA  for  now)  */ 

struct  MFI_PIA 
i 

unsigned  short  pra;  ./*  port  register  A  -  data  direction  A  */ 

unsigned  short  era;  /*  control  register  A  */ 


unsigned  short  prb; 

/*  port 

register 

B  - 

data  direction  B  */ 

^]-.signed  short  cm; 

/*  control 

register 

B 

*/ 

/*  4  Channels 

unsigned  char 

of  DAC  ADA-1  DAC 
*daci_a  =  (unsigned 

char 

*)  DACl-ADDR; 

*/ 

/•*'  8  Channels 

unsigned  char 

of  DAC  DAC-2E 
*dac2b_a  =  (unsigned 

char 

DAC2B-ADDR; 

*  / 

/*  16  Channels 
unsigned  char 

:  of  ADC  ADA-1 
^adcl_a  =  (unsigned 

char 

*)  ADCl-ADDR; 

*/ 

/*  16  Channels 
unsigned  short 

of  ADC  ADC- 2 
*adc2_a  =  (unsigned 

short 

*)  ADC2-ADDR; 

unsigned  short 

*via0  =  (unsigned 

short 

*)  VIAO-ADDR; 

int 

teleinetry_records_saved  = 

0; 

int 

inission_leg_counter 

=r 

0; 

int 

replication_count 

= 

1; 

int 

end_test 

FALSE; 

int 

wrap^count 

= 

0; 

double 

t 

= 

0.0; 

double 

dt 

= 

0.1; 

double 

rpm 

= 

0.0; 

double 

speed-limit 

= 

0.0; 

double 

main— motor— del tal 

0.0; 

double 

main— motor— del ta2 

= 

0.0; 

int 

main— motor— volt 1 

= 

512; 

int 

main-motor— volt2 

= 

512; 
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double 

X 

0.0; 

double 

y 

= 

0.0; 

double 

z 

0.0; 

double 

phi 

= 

0.0; 

/* 

degrees 

double 

theta 

= 

0.0; 

/* 

degrees  *! 

double 

psi 

= 

0.0; 

/* 

degrees  */ 

double 

x_dot 

= 

0.0; 

double 

y_dot 

= 

0.0; 

double 

z_dot 

0.0; 

double 

phi_dot 

= 

0.0; 

/* 

degrees/sec  */ 

double 

theta_dot 

0.0; 

/* 

degrees/sec  *! 

double 

psi_dot 

= 

0.0; 

/* 

degrees/sec  */ 

double 

speed 

0.0; 

double 

u 

= 

0.0; 

double 

V 

0.0; 

double 

w 

= 

0.0; 

double 

p 

= 

0.0; 

degrees/sec  */ 

double 

q 

= 

0.0; 

/* 

degrees/sec  */ 

double 

T” 

0,0; 

/* 

degrees/sec  */ 

double 

delta_planes 

= 

0.0; 

/* 

degrees  *! 

douL'je 

delta^rudder 

= 

0.0; 

/* 

degrees  */ 

double 

pcrt_rpiTi 

= 

0; 

double 

stbd_rpm 

= 

0; 

double 

vertical„thruster_volts 

= 

0,0; 

double 

lateral_thruster_volts 

= 

0.0; 

unsigned  snort 

psi_bi t_oid 

= 

0; 

double 

dg_cf f set 

= 

0.0; 

double 

k_psi 

0-0; 

double 

= 

0.0; 

double 

k_v 

= 

0.0; 

double 

k_z 

0,0; 

double 

k_w 

0.0; 

double 

k_theta 

= 

0.0; 

double 

)^_q 

= 

0.0; 

-3 - U  -  ^ 

k_thruster_psi 

0.0; 

double 

k_thruster_r 

= 

0.0; 

double 

k_thruster_rotate 

= 

0.0; 

double 

k_thruster_lateral 

= 

0.0; 

double 

k_thruster_z 

= 

0.0; 

double 

k_thruster_w 

= 

0,0; 

double 

k^propel ler_hover 

0.0; 

double 

k_surge_hover 

= 

0.0; 

double 

k_thruster_hover 

0.0; 

double 

k_sway_hover 

= 

0.0; 

double 

k_sigina_r 

12.0; 

double 

k_sigma_psi 

= 

28.87 

/ 

double 

Gta_steering 

= 

0.1; 

double 

sigma 

0.0; 

int 

mission_legs_total 

=: 

0; 

/*  values 

initialized  in  pa rse_iTjiss ioniser ipt^commands  ()  */ 

double 

t  i  ine_nex  t_c  omma  nd 

= 

0.0; 

double 

psi^command 

0.0; 

/* 

degrees  */ 

double 

p  s  i_c  onimand_h  o  ve  r 

0.0; 

/* 

degrees  */ 

double 

x^command 

= 

0.0; 

/* 

feet  */ 

double 

y_command 

= 

0.0; 

/* 

feet  */ 

double 

z^command 

= 

0.0; 

/* 

feet  */ 
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double 

s  t  b  d_  r  piTi_c  omrr,  and 

doub] e 

p  o  r  t_r  prri_c  omm  a  n  d 

= 

double 

p  1  a  n  e  s_c  omiTi  a  n  d 

= 

dr/ubie 

r  u  dde  1  _c  omiTi  a  nd 

= 

double 

rot  at  e_c  omit  and 

double 

1  a  t  era  l_coiTuttand 

= 

double 

bow_lateral_thruster_cominand 

= 

double 

sternal  at  era  l_thruster_comrriand 

= 

double 

bow_ve  rtical_thruste  r_c  omm  and 

= 

d  c.'  u  b ..  e 

stern_vert ical_thruster_command 

double 

waypoint_di stance 

double 

waypoint_angle 

= 

double 

track_angle 

— 

double 

al  on  g_track_di  stance 

= 

double 

cross_t racked! stance 

double 

standoff_di stance 

double 

depth_error ; 

inz 

roll__rate_G 

int 

pi tch_rate_0 

= 

I  no 

yaw  rate_0 

I  Ti  C 

roll_0 

1  r.  c 

pi tch_0 

..  I'i  I 

2„val0 

= 

iLfit 

swl 

= 

int 

error 

= 

int 

range 

= 

I  I'l  t 

oad_rng 

= 

I  Oi  L 

oa  d _ upda  t  e  s 

= 

IWZ 

i'ange_inde>: 

double 

range! 

= 

d  G  u  b  e 

ranges 

= 

double 

error! 

= 

douole 

error2 

dcuol e 

avg_rng 

Int 

k_range 

= 

in: 

rang e_ar ray  [3000]  ; 

in: 

pointer 

= 

int 

speed_a r ray  111] ; 

in: 

PortAFlag 

= 

int 

tick 

int 

curr_tick 

= 

int 

tick! 

int 

tick2 

= 

int 

i 

= 

int 

mask 

long 

davedate 

= 

long 

davetime 

= 

double 

value 

= 

short 

day 

char 

answer 

= 

int 

start^dwell 

= 

int 

socket_descriptor 

int 

socket_acc€pted 

= 

int 

socket_stream 

int 

socket_length 

= 

0.0; 

0.0; 

0.0;  /*  degrees  */ 

0.0;  /*  degrees  */ 

0.0;  /*  degrees/sec  */ 

0.0;  /*  ft/sec  */ 


0.0;  /*  volts  -24.-24  */ 

0.0;  /*  volts  -24. .24  */ 

0.0;  /*  volts  -24. .24  */ 

0.0;  /*  volts  -24. .24  */ 

0.0; 

0.0; 

0.0; 

0.0; 

0.0; 

10.0; 


0; 

0; 

0; 

0; 

0; 

0; 

0; 

0; 

0; 

0; 

0; 

0; 

0.0; 

0.0; 

0.0; 

0.0; 

0.0; 

0; 


NULL; 


0.0; 

0; 

0; 

0; 

0; 

0; 

OxOOOOff ff ; 

0; 

0; 

0.0; 

0; 

/  f  * 

0; 

0; 

0; 

0; 

255;  /♦  max  allowed  packet  size  */ 
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int 

bytes_received 

=  0; 

int 

t>ytes_read 

=  0; 

int 

bytes_written 

=  0; 

int 

bytes_left 

=  0; 

int 

by tes_sent 

=  0; 

char 

buffer_array  [FILEBUFFERSIZE] [256] 

char 

buffer 

[300]; 

int 

buf fer^size 

=  0; 

int 

buf f er_index 

=  0; 

int 

variables_parsed 

=  0; 

char 

buff  er_recei ved 

[300] , 

virtual_world_reinote. 

host_name  [60 

command^buf  fer 

[300]; 

FILE 

*  netstat_f i lept r ; 

not  impleinented 


struct  sockaddr_in  server_address ; 
struct  hostent  *server_ent i ty ; 


c  r.  a  r 


:  i  L 

int 

char 


double 


cioc 
cl  cc 


keyword  [81]; 

email^address  [81]  ; 

shutdowri_signal_received  =  FALSE; 
virtual_world_socket_opened  =  FALSE; 


ptr_index; 

print_help 


=  FALSE; 


speed_per_rprTi  =  2.0  /  700.0 


previousloopclock  =  0; 

currentloopclock  =  0; 


/*  steady  state: 

2.0  feet/sec  per  700  rpit  *! 


24V  <=>  +-2  lb,  +  Volts  moves  thruster  in  4  direction,  all  identical  */ 


dcuLle 

AUV_bow_ver  t i c  a 1  =  0.0; 

/* 

thruster 

rpm 

*/ 

double 

AlFV_stern_vertical  =  0.0; 

/* 

thruster 

rpm 

*/ 

double 

AlT'v  _bow_l  ateral  =  0.0; 

/* 

thruster 

rpm 

*/ 

double 

AlTv^s  t  e  r  n_l  ateral  =  0.0; 

/* 

thruster 

rpm 

*/ 

/*■  warning:  do  not  use  leading  zero  with  bearings  or  else  read  as  octal 
double  AlT^^STl 000_bearing  =  0.0;/*  ST_1000  conical  pencil  bearing  */ 

double  AL^'_ST1 00C_range  =  10.0;/*  ST_1000  conical  pencil  range  ♦/ 

double  AW_STi  000_st  rength=  20.0;/*  ST_1000  conical  pencil  strength*/ 


double  AL"v’_ST72S_bearing  = 
double  AUV_ST725_range  = 
double  AUV_ST725_strength  = 


90.0;/*  ST_725  1  x  24 
20.0;/*  ST.725  1  x  24 
10.0;/*  ST_725  1  X  24 


sector  bearing  */ 
sector  range  */ 
sector  strength  */ 


int  audible_command  =  TRUE; 

int  auvscriptfileguit  =  FALSE; 


t*****»**»******< 
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F.  globals.h  Global  Variable  Define  File  to  Permit  Multiple  References 


Program : 

Authors : 

sed  : 

System : 
Compiler : 

Compilation : 

[68020] 

[68030] 

[Irix  ] 

Purpose : 


globals.h 
Don  Brutzman 
28  October  94 

AUV  Gespac  68020/68030,  OS~9  version  2.4 
Gespac  cc  Kernighan  &  Richie  (K&R)  C 

ftp>  put  globals.h 
auvsiml>  chd  execution 
auvsiinl>  make  -k2f  execution 
auvsiml>  make  execution 

fletch>  make  execution 

Allows  repeated  use  of  global  variables  global. c  via  global. h 
in  order  to  prevent  compiler  warnings 


^^.’iclude  "defines. h 


^  -  f  del  1  red  ■'  G1jOHA,LS__H  } 

^ e ^  se 

# define  GLOBALS_H 

/*  Program'  configuration  flags 


*/ 


extern 

int 

TRACE 

extern 

int 

DISPLAYSCREEN 

extern 

int 

LOCATTONLAB 

extern 

inr 

TACTICAL 

extern 

int 

LOOPFOREVER 

extern 

int 

LOOPFILEBACKUP 

extern 

int 

PARALLELPORTTRACE 

extern 

int 

SONAR INSTALLED 

extern 

int 

SONARTRACE 

extern 

int 

ENTERCONTROLCONSTANTS 

extern 

int 

REALTIME 

extern 

int 

DEADRECKON 

extern 

int 

DEADSTICKRUDDER 

extern 

int 

DEADSTICK PLANES 

extern 

int 

SLIDINGMODECOURSE 

extern 

int 

THRUSTERCONTROL 

extern 

int 

ROTATECONTROL 

extern 

int 

LATERALCONTROL 

extern 

int 

FOLLOWWAYPOINTMODE 

extern 

int 

WAYPOINTCONTROL 

extern 

int 

HOVERCONTROL 

#if  def 

ined 

(sgi  ) 

extern 

#else 

int 

EMAIL 

extern 

int 

EMAIL 

;  /*  l=trac€  on,  0=trace  off  */ 
;  /*  l=screen  on,  0=screen  off  */ 
;  /'*  l=graphics  lab,  0=actual  vehicle  ♦/ 
;  /*  l=tactical  on,  0=tactical  off  ♦/ 
;  /*  l=_repeat  execution  indefinitely  */ 
;  /*  l=backup  files  between  replications*/ 

;  /*  l=trace  each  char  received  at  port  */ 
;  /*  l=sonar  head  available  for  query  */ 
;  /*  l=trace  on,  0=trace  off  */ 
;  /*  l=manual  entry,  0=default  values  */ 


!*  1=1  second  real-time  waits,  0=none  */ 


/*  l=dead  reckon  navigate,  0=regular  */ 
/*  l=use  ordered  rudder,  0  =  control  */ 
!*  l=use  ordered  planes,  0  =  control  */ 
/*  l=use  sliding  mode,  0  =  control  */ 


I* 

/* 

/* 

/* 

/* 

/* 


l=use  thrusters,  0=use  propellers  */ 
l=us€  thrusters  to  rotate  in  place  */ 
l=use  thrusters  for  lateral  motion  */ 
1=  go  to  WAYPOINT  without  WAITS  */ 
1=  go  to  WAYPOINT  *  / 
l=hover  at  WAYPOINT  ♦ / 


;  /*  1-send  e-mail,  0=don't  send  e-mail  */ 
;  /*  can't  send  e-mail  via  OS-9  directly  */ 
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#endi  f 


extern 

int 

NOT_YET„RE IMPLEMENTED 

;  /* 

extern 

double 

TIMESTEP 

;  /" 

/* 

extern 

int 

KEYBOARDINPUT 

;  /* 

^♦♦★♦★*-*************»****^***********’**** 

/*  file?  and  paths 

extern  FILE  *  auvscript f i le; 
extern  FILE  auvordersf ile; 
extern  FILE  *  auvdatafile; 
extern  FILE  *  auvtextfile; 
extern  FILE  *  controlconstantsf ile; 
extern  FILE  *  eitailaddressf ile ; 

*  FILE  ’  serialtest f ile;  */ 

extern  int  serialpath  ; 

extern  int  sonarpath 


code  in  block  needs  reverification  */ 

time  of  a  single  closed  loop  */ 

add  code  to  warn  if  exceeded  ««  */ 

l=read  keyboard  vice  mission  file  */ 

*/ 


Variables  and  data  structures 


*  ★ 


★  ★★**<t********************** 


*/ 

V 


buffers  of  full  strings  for  byte  transfer  to  tactical  level  &  disk  file  */ 
/*  'buffer'  usually  <  256,  intentionally  oversized  in  case  of  overflow  error  */ 


extern  time_t  system_time  ; 

extern  struct  tm  *system_tmp  ; 

/*  partial  structure  template  for  the  MFI  (only  interested  in  PIA  for  now)  */ 


struct  MF: 
( 

unsigned 

unsigned 

unsigned 

uns^aned 

}; 


:_PIA 

short  pra; 
short  era; 
short  prb; 
short  erb; 


!*  port 
/*  control 
/*  port 
/*  control 


register  A 
register  A 
register  B 
register  B 


/»  4  Channels  of  DAC  ADA-1  DAC 


extern 

unsigned 

char 

*dacl_a 

8  Channels  of  DAC 

DAC-2B 

extern 

unsigned 

char 

*dac2b_a  ; 

/»  16 

Channels 

of  ADC  ADA-1 

extern 

unsigned 

char 

*adcl_a 

/*  16  Channels  of  ADC 

ADC- 2 

extern 

unsigned 

short 

*adc2_a  ; 

extern 

unsigned 

short 

*via0  ; 

extern 

int 

telemetry_records_saved  ; 

extern 

int 

mission_leg_counter ; 

extern 

int 

replication_count  ; 

extern 

int 

end_test  ; 

extern 

int 

wrap_count  ; 

extern 

double 

t 

extern 

double 

dt 

extern 

double 

rpm; 

extern 

double 

speed_limit  ; 

data  direction  A  */ 
*  I 

data  direction  B  */ 


♦/ 


*/ 


*/ 
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evreri'i 

double 

main_niotor_deltal 

extern 

double 

ma  i  n_ino  t  o  r__de  1 1  a  2 

ex  tern 

int 

ma  i  n_Tno  t  o  r_vc  1 1 1 

extern 

int 

main_motor_vol t2 

ext  ern 

double 

X 

extern 

doubl e 

y 

extern 

double 

2 

extern 

double 

phi 

degrees 

*/ 

extern 

double 

theta 

/* 

degrees 

*/ 

extern 

double 

psi 

/* 

degrees 

*/ 

extern 

double 

x_dot 

extern 

double 

y_dot 

extern 

double 

z_dot 

extern 

double 

phi_dot 

/* 

degrees/sec 

*/ 

ext  err. 

double 

theta_dot 

/* 

degrees/sec 

extern 

double 

psi_dot 

/* 

degrees/sec 

*/ 

extern 

double 

speed 

extern 

doubl  e 

u 

extern 

double 

V 

extern 

double 

w 

extern 

double 

p 

/* 

degrees/sec 

extern 

double 

q 

/* 

degrees/sec 

extern 

double 

r 

/* 

degrees/sec 

*/ 

extern 

double 

del ta_planes 

/* 

degrees 

»/ 

extern 

double 

del ta_rudder 

/* 

degrees 

*/ 

extern 

double 

port_rprr 

p  V  ^  p  r; 

double 

stbd^rpTTi 

extern 

double 

vertical_thruster_vol ts 

extern 

double 

iateral_thruster_vol ts 

extern 

unsigned 

short  psi_bit_old 

extern 

double 

dg_offset 

extern 

double 

; 

extern 

Qcu  bi e 

K  r  ; 

extern 

0  ou  bi e 

i 

extern 

double 

k  2  ; 

extern 

douDle 

k_W  ; 

extern 

double 

k_theta  ; 

extern 

double 

k_q  ; 

extern 

double 

k_thruster_psi  ; 

extern 

double 

k_thruster_r  ; 

extern 

double 

k_thruster_rotate  ; 

extern 

double 

k_thruster_lateral  ; 

extern 

double 

k_thruster_2  ; 

extern 

double 

k_thruster_w  ; 

extern 

double 

k_propeller_hover  ; 

extern 

double 

k_surge_hover  ; 

extern 

double 

k_thruster_hover  ; 

extern 

double 

k_s  way  _h  over  ; 

extern 

double 

k_sigma_r 

extern 

double 

k^sigma^psi  ; 

extern 

double 

eta_steering  ; 

extern 

double 

sigma  ; 

extern 

int 

inission_legs_total  ; 

/* 

values 

initialized  in  parse_mission_script 

..commands  ( ) 

*/ 

ext  err. 

double 

time_next_coinmand  ; 
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ext err  double 
extern  double 
extern  double 
extern  double 
extern  double 
extern  double 
extern  double 
extern  double 
extern  double 
extern  double 
extern  double 


extern 

double 

bow 

_lateral_thruster  command 

extern 

double 

stern 

_lateral_thruster  command 

extern 

double 

bow_ 

vertical_thruster_command 

extern 

double 

stern_ 

vertical_thruster_command 

extern 

double 

waypoint_di stance 

ex  tern 

double 

waypoint_angle 

extern 

double 

track_angle 

ex  t  e  rn 

d  ou  Lie 

a long_track_di stance 

extern 

GGU  o  e 

cross_track_di stance 

extern 

double 

standcff_di stance 

ex-trin 

doubl e 

depth_error ; 

extern 

int 

roll_rate_0 

extern 

r  nt 

pitcher a te_0 

extern 

int 

vaw_rat€_0 

extern 

int 

rol 1_0 

extern 

int 

pitch_0 

extern 

int 

2_val 0 

extern 

ji  nt 

swl 

extern 

int 

error 

extern 

int 

range 

extern 

int 

bad_rng 

extern 

int 

bad_updates 

extern 

int 

range_index 

extern 

double 

rangel 

extern 

double 

range2 

extern 

dcubl  e 

errorl 

ex  ten' 

doubl  e 

error2 

extern 

d  ou  1j  j.  e 

avg_rng 

ex  t  e  i 

int 

k_range 

extern 

int 

range_array  [3000] ; 

extern 

int 

pointer 

extern 

int 

speed_array  [11 j 

extern 

i  nt 

PortAFlag 

extern 

int 

tick 

extern 

int 

curr_tick 

extern 

int 

tickl 

ex  t  e  rn 

int 

tick2 

extern 

int 

i 

extern 

int 

mask 

extern 

long 

davedate 

extern 

long 

davetime 

extern 

double 

value 

extern 

short 

day 

extern 

char 

answer 

extern 

int 

start_dwell 

psi_coininand 
p  s  i_c  omiriand^hover 
x_cominand 
y_command 
z_comjTiand 
s  t  bd_r  pm_c  oinina  nd 
port_rpm_conmand 
pi anes_command 
ru  dd  e  r_c  omma  nd 
r o t a t e_c omm a nd 
lateral  coimriand 


/*  degrees  */ 

degrees  */ 

/*  feet  */ 

/*  feet  */ 

/*  feet  */ 

/*  degrees  */ 

/*  degrees 
/*  degrees /sec  */ 

/*  ft/sec  */ 


volts  -24.  .24  */ 
/♦  volts  -24  .  .24  ’"Z 
/♦  volts  -24 .  .24  */ 
/*  volts  -24 .  .24  */ 


extern  int 


socket^descriptor 


socket^accepted 

sockec_streain 

socket_length  ; /*  max  allowed  packet  size  */ 

by tes^received 
bytes_read  ; 

bytes_written  ; 

byces_left 
bytes^sent 

buffer_array  [FILEBUFFERSIZE] [256] ;  --  not  implemented  */ 

buffer  [300]; 

buffer_si2e 

buffer_index  ; 

variables_parsed 

buf f€r_received  [300], 

vi rtual_world_remote_host_name  [60] , 
command_buf fer  [300] ; 

*  netstat_f ileptr ; 


extern  char  keyword  [81]; 

extern  char  email_address  [81]; 

extern  int  shu tdown_signal_received 

extern  int  virtual_world_socket_opened  ; 

extern  char  *  ptr_index; 

extern  int  print_help; 

extern  double  speed^per^rpm;  /*  steady  state:  2,0  feet /sec  per  700  rpm  */ 


/»  -f-  24V  <=>  +-2  lb,  +  Volts  moves  thruster  in  +  direction,  all  identical  */ 
extern  double  AUV_bow_vertical  ;  /*  thruster  rpm  */ 

extern  double  AU7_stern_vert ical  ;  /*  thruster  rpm  */ 

extern  double  AW_bow_lateral  ;  /*  thruster  rpm  */ 

extern  double  AUV_stern_lateral  ;  /*  thruster  rpm  */ 

/*  warning:  do  not  use  leading  zero  with  bearings  or  else  read  as  octal  */ 
extern  double  AUV_ST1000_bearing  ;  /*  ST_1000  conical  pencil  bearing  */ 

extern  double  AUV_ST1000_range  ;  /♦  ST_1000  conical  pencil  range  * ! 

extern  double  AUV_ST1000_strength;  /*  ST_1000  conical  pencil  strength*/ 

extern  double  AUV_ST725_bearing  ;  /*  ST_725  1  x  24  sector  bearing  ♦/ 

extern  double  AUV_ST725_range  ;  /*  ST_725  1  x  24  sector  range  */ 

extern  double  AUV_ST725_strength  ;  /*  ST_725  1  x  24  sector  strength  */ 

extern  int  audible^command 

extern  int  auvscriptf ilegui t 

#endi f 


extern 

int 

extern 

int 

ex*“err. 

int 

extern 

int 

extern 

int 

extern 

int 

extern 

int 

extern 

int 

char 

extern 

char 

exterij 

int 

extern 

int 

extern 

int 

extern 

char 

extern 

FILE 

extern 

struct 

extern 

struct 

extern 

char 

e  >1 1  e  r  r. 

char 

ex  tern 

int 

extern 

int 

extern 

char 

extern 

int 

extern 

double 

extern 

cl ock_t 

ex  t  e  I'l'i 

ClGCK_t 

/»  24V  <=> 

extern 

double  . 

extern 

double  . 

extern 

double  . 

extern 

double  . 

/*  warning:  do 

extern 

double  . 

extern 

double  i 

extern 

double  i 

extern 

double  i 

extern 

double  i 

extern 

double  j 

extern 

int  j 

extern 

int  i 
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MakefUe.OS9  for  Execution  Level  under  OS-9  Operating  System 


#  AUV  execution  level  Makefile. 0S9 

^  Don  Brutzman  27  OCT  94 

modified  OS-9  makefile  (originally  from  /hO /INET/ SOURCE / ) 

#  this  Makefile  is  necessary  to  resolve  network  libraries  compilation 

#  Note  that  OS-9  Make  syntax  is  nonstandard. 

DD  =  /hO 

LIE  =  -1=$ (DD) /lib/sys. 1 

SOCKLIB  =  .  . . /lib/socklib.l 

NETLIE  =  . . . /lib/netdb.l 
#R:IR  =  RELS 

P  *  R  = 

ODZR  =  $(DD)/cmds 

RFLAGS 

CFLAGS  =  -bp  -g 

SOCr.GE  =  recv^str  xnat^str 

PSOURCE  =  recv_3tr.r  xir.it_str.r 

make -date:  $ (SOURCE) 

touch  ma^’e .  da^e 

olcbals:  $(SOCKLI5.  $ (NETLIB)  globals.c  globals.h  defines. h  Makefile 

cc  $(CFLAG£)  globals.c  -r  -1= ../$ (NETLIB)  -1= ../$ (SOCKLIB)  $(LIB) 

pa: se_f unctions :  $(SOCKLIB;  $ (NETLIB)  parse_functions . c  globals.h  defines. h 
Makefile 

cc  $ 'CFLAGS)  parse^functions.c  -r  -1= ../$ (NETLIB)  -1= . . /$ (S(XKLIB)  $(LIB) 

execution:  $ (SOCKLIB)  $ (NETLIB)  globals.r  parse_functions . r  execution. c 
globals.h  defines. h  Makefile 

cc  $ (CFLAGS)  execution. c  globals.r  parse_functions . r  -f=execution  \ 

-1= ../$ (NETLIB;  - 1= ../$ (SOCKLIB )  $(LIB)  -Im 

^  clean: 

M  rm.  *.r  /hO/CMDS/executi on 

^  old  versions 
^  -K=C'  dBC'Ou  (default) 

#  -k=2  66020 

#  -t=/r0  puts  temporary  files  in  ramdisk  and  speeds  compilation 

#  $».r  saves  relocatable  modules  in  RDIR 

#  $ (SOURCE):  $ (SOCKLIB)  $ (NETLIB) 

^  CC  $(CFLAGS}  $^.r  -f=$*  - 1= ../$ (NETLIB )  -1= ../$ (SOCKLIB)  $ (LIB) 

#  CC  $ (CFLAGS)  execution. c  -r  -1= ../$ (NETLIB )  -1= ../$ (SOCKLIB)  $(LIB) 


os9 sender  os9 server 

os9sender.r  os9server.r 
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H.  Makefile.SGI  for  Execution  Level  under  SGI  Irix  5.2  Operating  System 

#  A’/v  execution  level  Makefile,  sgi 
^  Don  Brutzman  27  OCT  94 

execution:  globals.c  parse_functions . c  execution. c  globals.h  defines. h  Makefile 
cc  -g  “Im  -cckr  -w  \ 

globals.c  parse_functions . c  execution. c  -o  execution 

#  The  'warnings'  make  option  gives  voluminous  diagnostics  which  are  useful 

#  in  preventing  mysterious  bugs  when  the  execution  code  is  ported  to  OS-9. 

^  Similar  to  lint. 


warnings : 

cc 


-g  -Im  -cckr  -fullwarn  -wlint,p  \ 

globals.c  parse_f unctions . c  execution. c  -o  execution 


touch  globals.c  parse_functicns . c  execution. c 
make  execution 


m,  execution  *.o 
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1.  startup  OS-9  Reboot  System  Configuration  File 

- 1  -np 

* 

*  OS- 9  startup  file 

*  Last  inodi f ication  22  JUN  94  Walt  Landaker  &  Don  Brutzman 


setime  -s  ^  start  systeiu  clock 

link  shell  cio  make  •shell"  and  'cio"  stay  in  memory 


the  following  are  already  i 

iniz  rO  hC  dC  tl  pi 

load  utils 

load  hoot obj s /dd . rO 

jn:  r  .  ramdisk  >/nil  »/nil& 


n  RAM  memory  from  hard  disk  boot file  /hO/OS9boot 
initialize  devices 

;*  make  some  utilities  stay  in  memory 
;*  get  the  default  device  descriptor 
;*  initialize  it  if  its  the  ram  disk 


tsmon  /tl  & 


start  other  terminals 


setenv  TEW4  vtlOO 

start  Ian 

wait 


list  sys 'mctd 

shel  1  term  -e=  /hO  /sys /erririsg .  short 
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login  OS-9  User  Login  and  Unix  Alias  File 


#  iuvsiiT:;  .login  file 

^  for  OS-9  with  ' sh '  shell 

#  Don  Brutzman  22  June  94 
history  40 


Srrtenv  T£P>:  v 

tioo 

alias 

cat 

list 

alias 

cd 

chd 

alias 

chmod 

a  1 1  r  -  ? 

alias 

clear 

els 

alias 

cp 

copy 

alias 

h 

history 

al  I  as 

1 

dir  -ade 

3  - 1  a  s 

1  s 

dir  -ad 

alias 

m 

list 

alias 

iTian 

help 

alias 

mkdi  r 

makdi  r 

alias 

TTiore 

list 

alias 

lo 

logout 

alias 

nopause 

tmode  nopause 

a  ^  I  a  s 

pause 

tiTiode  pause 

alias 

ps 

procs 

alias 

pwd 

pd 

alias 

quit 

logout 

all  as 

reboot 

shell  break 

alias 

ren 

rename 

a  ^  _  as 

riT 

del 

al  i  as 

riTid  I  r 

deldi r 

alias 

s 

sh 

alias 

source 

Sh 

alias 

type 

list 

#  following  doesn't  seeir,  to  work 

#  automatically  on  telnet  logins 
tmode  nopajse 


ecno  "telnet  users  type:  nopause" 
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auv _ploLgnu:  Execution  Level  Telemetry  Plotting  using  gnuplot 


« 


iiir'name:  auv_plot.gnu 


function : 


GNUPLOT  V3.5  script  to  plot  AUV  telemetry  data 
to  screen  U  to  PostScript  files 


updated:  18  OCT  94 

author:  Don  Brutzman 

execution:  gnuplot>  load  ‘auv^plot .gnu* 
gnuploo  reread 

unix>  gnuplot  auv_plot.gnu 


ing : 

' xpsview' 
gh«:istview 


-wp  “Skipc  -or  landscape  -/ execution/ AUV_telemetry.ps  &  <1 

-landscape  -/ execution/ AUV_telemetry.ps  4  # 


:  program  call:  system  ("gnuplot  auv_plot .gnu" ) ;  # 

telemetry:  mission . output . telemetry  (AUV  telemetry  0.1  sec  interval)# 
alt^^rnate:  mission  .output .  l_second  lAUV  telemetry  1.0  sec  interval)# 

# 

alternate  plot:  unix>  gnuplot  auv_jDlot_l_second.gnu  # 

# 

output  files:  AU\'_teienietry .ps  4  *.eps  plots  # 


related  files: 


execution . c 

underwater  virtual  world 


vjtput  archive:  ftp://taurus.cs.nps.navy.mi1/pub/auv/AUV_telemetry.ps.2  # 


gnuplot  FAO: 


dist  riljut  ion : 


f  tp : // f tp . dartmouth . edu/pub/gnuplot /faq/gpt_faq .html 
http : //WWW. cs .dartmouth .edu/gnuplot/ 


«  Piot  filenames: 

>  combined  AU'v^^telemetry -ps 

#  figure  1  AUV_x_y.eps 

#  figure  2  AU\'_t_x.eps 

#  figure  3  Air/_t_y.eps 

#  figure  4  AUV_t_z.eps 

#  figure  5  AUV_c_pha.eps 

#  figure  6  AUV_t_th€ta .eps 

#  figure  7  AUV_t_theta_ali.eps 

#  figure  8  AU\/’_t_psi  .  eps 

#  figure  9  AUV_t_lateraI_thrusters.eps 

#  figure  10  AUV_t_vertical_thrusters.eps 

#  figure  11  Ai^’_  t  _u .  ei:>s 
i  figure  12  AUV_t_v.eps 

#  figure  13  AUV_t_w.eps 

#  figure  14  AUV_t_p.eps 

#  figure  IS  AU'v^_t_q.eps 
i  figure  16  AUV_t_r.eps 

#  figure  17  AU\Lt_delta_rudder.eps 

#  figure  18  AUV_t_delta_planes.eps 
«  figure  19  AUV_t_rpm.eps 

#  figure  20  AUV_t_thrusters.eps 


set  terminal  xli 


set  time 
set  grid 
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set  data  style  linespcints 

set  samples  10  #  data  point  plotting  frequency 

set  xlabel  “East  ->  (y^world)  (ft]“ 
set  ylabel  "North  ^  {x_world)  [ft]" 

set  title  "NPS  hUV  telemetry  1"  26,. 8 

l::l.  -I^^rlemetry-  using  4:3  title  ‘x  vs  y  (geographic  position  plot] 

paus-  -1  •hit  enter  to  continue  with  plot  2  "  j'  y  y 

Set  xlabel  “time  t  (seconds! “ 
set  y label 


set  title  “NPS  ALA/  telemetry  2"  26,  .8 

plot  “mission . output . telemetry “  using  2:3  title  “t  vs  x  [ft] 

mission. output .telemetry  using  2:15  title  “t  vs  x  dot  [ft/sec] 

nit  enter  to  continue  with  plot  3  ^ 


er  title  “NP?  AW  telemetry'  3“  26,. 8 

-■■-'t  "m:  ssic/t. .  output .  telemetry  “  using  2:4  title  "t  vs  y  [ft] 

•mission. output .teiemetry“  using  2:16  title  “t  vs  y_dot  [ft/sec] 

r-.:se  -j.  “hit  enter  to  continue  with  plot  4  • 


set  title  “NPS  AW  telemetry'  4“  26,  .8 

p-.:t  ^m^ssion. output  .telemetry  using  2:5  title  "t  vs  z  (depth)  [ft]  •  \ 

rr..ss^or. .  output .  telemet  ry“  using  2:17  title  “t  vs  z  dot  (depth  rate)  fft/secl"'  \ 

iii 2=22  tuie  -t  vs  diltaJlaLs  idlgf  •;  \ 

using  1:j  title  't  vs  ordered  depth  [ft]  •  with  st^ 

pau=-  -1  hit  enter  to  continue  with  plot  5  •  i  j  wiun  st^ 

set  title  “NPS  Airy  telemetry  5“  26,. 8 

'onrSnr VS  phi  (roll:  X  axis)  [deg]  •  \ 

rau<=P  J^if--5-output.  telemetry  using  2:18  title  “t  vs  phi.dot  (roll  rate)  deg/sed  “ 

ra  use  •"  j.  n  j.  enr  ro  nor;  r*,  mici  i.Tirv  t-vI  <!■ 


t  vs  z  (depth)  [ft] 

- ^  t  vs  z_dot  (depth  rate)  [ft/sec]' 

using  2:22  title  "t  vs  delta^lanes  [deg] 

using  1:3  title  't  vs  ordered  depth  [ftj^ 


with  steps 


“mission .output . telemetry  usinc 

pause  -1  “hit  enter  to  continue  with  plot  6^ 


Sr't  title  “NPS  ALA'  telemetry  6“  26,. 6 
r-l:  t  •mission  .output .  telemetry  •  u 

•mission .output . telemetry  u 

p.ause  -I  “hit  enter  to  continue  with  plo 


using  2:7  title  "t  vs  theta 


set  title 
ploL  "mi 


Ltle  "NPS  ALA^  telemetry  7- 
•mission  .output .  telemetry 
“mission. output . telemetry “ 
“mission .output . telemetry 
•mission . output . telemetry 
“mission,  outp-ut  .tel erne try“ 
“mission  .outt»ut . orders “ 


using  2:7 


•t  vs  theta 


(elevation  angle) 


using  2:^9  title  “t  vs  theta_dot  (elevation  rate) 


using  2:5  title 
using  2:11  title 
using  2:22  title 
using  1:3  title 


title  “t  vs  z  (depth) 

title  'c  vs  w  (heave) 

title  "t  vs  delta_planes 
title  “t  vs  ordered  depth 


“hit  enter  tc  continue  with  plot 


set  title  'NPS  ALPy  teiemtetry  8“  26,  .8 
p-.ot  ‘mission  .  output .  telemetry  “  usin 

“mission .output . telemetry  usin 

•mission -output . telemetry  usin 

•mission .output .telemetry  usin 

•mission. output . telemetry'  usin 

•mission .output .orders"  usin 

pause  -1  'hit  enter  to  continue  with  plot  9 

set  title  'NPS  AUV  telemetry  9*  26,. 8 
plot  "mission . output . telemetry ■  usini 

•mission .output . telemetry •  usini 

•mission. output .telemetry"  usini 

•mission . output . telemetry •  usini 

•mission. output  .telemetry  usini 

steps,  \ 

•mission . output . telemetry "  usinc 

steps,  \ 

•mission. output .orders"  usinc 


using  2:8  title  "t  vs  psi  (azimuth:  2  axis) 

using  2:20  title  “t  vs  psi_dot  (azimuth  rate) 

using  2:14  title  "t  vs  r  (yaw  rate) 

using  2:10  title  "t  vs  v  (sway) 


using  2:21  title  "t  vs  <3elta_rudder 
using  1:2  title  "t  vs  ordered  headino 

nr  Q  •  ^ 


using  2:8  title  "t 
using  2:20  title  't 
using  2:14  title  't 
using  2:10  title  "t 
using  2:27  title  -t 

using  2:28  title  "t 


vs  psi  (azimuth:  2  axis) 

vs  psi_dot  (azimuth  rate) 
r  (yaw  rate) 

vs  V  (sway) 

vs  bow  lateral  thruster 

vs  stern  lateral  thruster 


e)  [deg] 

\ 

?)  [deg /sec 

1  • 

j 

e)  [deg] 

\ 

)  [deg /sec 

]■! 

\ 

[ft] 

• 

\ 

[ ft/sec] 

m 

\ 

[deg] 

■ 

\ 

[ft] 

with 

[deg] 

\ 

[deg/sec] • 

\ 

I  deg/sec J • 

\ 

[  ft/sec] • 

\ 

[deg] 

\ 

[deg] 

with  steps 

[deg] 

\ 

(deg/sec] “ , 

\ 

[deg/sec] • , 

\ 

[  ft/sec]". 

\ 

pause  -1  'hit  enter  to  continue  with  plot  10 


using  1:2  title  -t  vs  ordered  heading 


[volts] 

[volts] 

[deg] 


•  with 

•  with 

■  with  steps 


set  title  -NPS  AUV  telemetry  10"  26, .8 

plot  •mission. output. telemetry  using  2:5  title  't  vs  2  (depth) 
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•mission .output .telemetry  using  2;11  title  *1  vs  w  (heave)  { ft/sec] •,  \ 

•m: ssicn . output . telemetry  using  2:25  title  't  vs  bow  vertical  thruster  [volts]  •  with  steps,  \ 

•mission. output .telemetry'  using  2:26  title  "t  vs  stern  vertical  thruster  [volts]  •  with  steps!  \ 

•mission. output .orders*  using  1:3  title  't  vs  ordered  depth  [ft]  •  with  steps' 

pause  -1  'hit  enter  to  continue  with  plot  11  • 

:iti*T  “NPS  AUV  telerrietry  11  •  26,. 8 

pior  •mission. output .telemetry  using  2:9  title  't  vs  u  (surge)  [ft/sec]* 

pause  -1  ’hit  enter  to  continue  with  plot  12  • 

set  title  'NPS  AUV  telemetry  12“  26,. 8 

f>:  ot  'mission  .output  .telemetry  using  2:10  title  't  vs  v  (sway)  (ft/secj* 

pause  -1  'hit  enter  to  continue  with  plot  13  • 

set  title  'NPS  AUV  telemetry  13*  26, .8 

plot  'mission. output .telemetry  using  2:11  title  't  vs  w  (heave)  [ft/sec]* 

pause  -1  'hit  enter  to  continue  with  plot  14  • 

set  title  'NPS  AUV  telemetry  14'  26,. 8 

plot:  'mission .output .  telemetry •  using  2:12  title  "t  vs  p  (roll  rate)  [deg/sec]  • 

pause  ~i  'hit  enter  to  continue  with  plot  15  • 

set:  title  'NPS  AUV  telemetry  15“  26,  .8 

r:""  'miEsior;. output  .telemetry  using  2:13  title  't  vs  q  (pitch  rate)  [deg/sec]' 

-I  "hit  en'ter  to  continue  with  plot  16  ' 

se"  title  'KPS  AUV  telemetry  16'  26, .8 

r - 'o  'ri SE ion . output . telemetry •  using  2:14  title  "t  vs  r  (yaw  rate)  [deg/sec]* 

paus-  Tirt  enter  to  continue  with  plot  17  • 

set  title  'NfS  AU^  telemetry  17'  26,. 8 

t^ot  'missitr.  .output .  telemetry  using  2:21  title  't  vs  delta_rudder  bow  [deg]' 

i:au3e  'hit  enter  to  continue  with  plot  16  • 

set  title  ’NFS  AU\^  telemetry  18'  26,. 8 

plot  'mission. output .telemetry  using  2:22  title  't  vs  delta_planes  bow  [deg]' 

pause  -1  “hit  enter  to  continue  with  plot  19  ' 

Set  title  'NPi?  A’JV  telemetry  19'  26,  .8 

{.lot  'mission  .output .  telemetry'  using  2:23  title  't  vs  rpm_left  [rpm]',  \ 

•mission .output .telemetry  using  2:24  title  't  vs  rpm„right  [rpm]',  \ 

•mission. output .orders'  using  1:4  title  't  vs  rpm^ordered  [rpm]'  with  steps 

pa.-.=e  -1  'hit  enter  to  continue  with  plot  20  ' 

set  title  'KPS  AUV  telemetry  20'  26,. 8 

tlor  'mi ssion .output . telemetry  using  2:25  title  't  vs  bow  vertical  thruster  [volts]'  with  steps,  \ 

•mission .output .telemetry  using  2:26  title  't  vs  stern  vertical  thruster  [volts]'  with  steps,  \ 

•iT-ission. output  .telemetry  using  2:27  title  't  vs  bow  lateral  thruster  [volts]'  with  steps,  \ 

•mission -output -telemetry  using  2:28  title  't  vs  stern  lateral  thruster  [volts] '  with  steps 

pause  -1  'hit  enter  to  plot  postscript  files  &  quit' 

#««#«««««« 

pause  0  'plotting  combined  plot  AUV_telemetry.ps  ...' 
c  1  ear 

t  - - 

t  uncomment  to  choose  one  of  the  following  two  options 

«  - 

60%  size  for  xpsview 

#  sec  size  0.6,  0.6 

#  set  terminal  postscript  portrait  color  'Courier'  14 

#  set  title  *NPS  AUV  telemetry'  29,. 8 

«  - 

»  full  page  for  printing 

set  terminal  postscript  landscape  color  'Courier'  20 
set  title  'NPS  AUV  telemetry'  16,-8 
« - - 

set  output  *AUV_telemetry ,ps' 

pause  C  'page  1* 

set  xlabel  'East  ~>  (y_world)  [ft]' 


■m 

« 

« 
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s*er  \'Iabel  ’North  ->  (x_worId}  [ft]’ 

Set  title  "NPS  AUV  teiemetry  1’  14,. 8 
plot  *1111551011.  output  .telemetry’ 


set  xiabel  ’time 
sec  ylabei 


{ seconds ! 


using  4:3  title  ’x  vs  y  (geographic  position  plot)’ 


pause  0  "page  2’ 

set  title  ’NFS  AUV  telemetry  2* 
plot  "mission. output  .telemetry^" 
"mission. output . telemetry’ 

t'ause  0  “F^age  3’ 

set  title  "NFS  AUV  telemetry  3" 
plot  "mission. output . telemetry ’ 
•mission. output .telemetry 

F>aus>-  G  -page  4“ 

Sr-r  title  “NriT  AUV  telemetry  4’ 
i:  ’mlssi or  .  lutput  .  telemetry 

•mission .output . telemetry" 
"mission . output . telemetry 
"mi  SSI on. output .orders’ 

pause  0  ’page  5’ 

set  titjLe  ’NFS  AUV  telemetry  5* 
P-i-^t  "mussion .  output .  telemetry* 

•  mi  s  s  i  on .  ou  tpu t .  t  e  1  erne t  ry  ’ 

pause  0  ’p^age  6’ 

se:  title  ’N.^S  AUv  telemetry  6’ 
p..Oc  “mission  .  output .  telemietry  ’ 
•mission .output .  telemetry 

pause  0  ’page  7- 

set  title  ’NFS  AUv’  telemetry  7* 
p lot  "mission . output . telemetry’ 
•mission. output . telemetry 
•mission . output . telemetry 
•mission.outpuc.telemetry 
‘mission .  outp^ut .  teiemetry 


’mil  ssicjn  .  output .  orders 


pause  0  ’page  6* 

set  title  ’NFS  AU\'  telemetry  8' 
Plot  ’mission .output . telemetry 
•mission .  outp*ut .  telemetry’ 
•mission . output . telemetry 
•mission .output . telemetry 
’mission  .output .  telemietry 
’.mission  .output  .orders’ 


PciuSt  U  ’pag^r  9’ 

title  "NFS  AU'*/  teiemetrv  9’ 
P  -  vt  "mission  .  output .  telemiet I'v ’ 
’mission. output . tel emet  ry • 
•mission .output . telemetry’ 
•mission .output . telemetry 
•mission .output . telemetry’ 
steps.  \ 

•mission .output . telemetry* 
steps,  \ 

•mission .output .orders’ 

pause  0  ‘page  10’ 
set  title  ’NFS  AUV  telemetry  10’ 
plot  "mission .output . telemetry 
•miss ion. output . telemetry 
•miss ion. output .telemetry 
•mission. output . telemetry 
•mission. output .orders" 

pause  0  ’page  11’ 

set  title  -NFS  AUV  telemetry  11" 

plot  •mission. output .telemetry 


using  2:3  title  't  vs  x  [ft] 

using  2:15  title  "t  vs  x_dot  [ft/sec] 


using  2:4  title  ’t  vs  y  [ft] 

using  2:16  title  "t  vs  y_dot  [ft/sec] 


using  2:5  title  ’t  vs  z  (depth) 

using  2:17  title  ’t  vs  2_dot  (depth  rate) 
using  2:22  title  ’t  vs  delta_planes 
using  1:3  title  ’t  vs  ordered  depth 


Itt]  \ 

[  ft/sec] • ,  \ 

[deg]  \ 

[fc]  •  with  steps 


using  2:6  title  ’t  vs  phi  (roll:  x  axis)  [deg] 

using  2:18  title  ’t  vs  phi^dot  (roll  rate)  [deg/sec 


using  2:7  title  ’t  vs  theta  (elevation  angle 

using  2:19  title  -t  vs  theta_dot  (elevation  rate) 


using  2:7  title  ’t  vs  theta  (elevation  angle 

using  2:19  title  ’t  vs  theta^dot  (elevation  rate) 

using  vs  z  (depth) 

using  2:ii  title  ’t  vs  w  (heave) 

usj.ng  2:22  title  ’t  vs  delta^lanes 
using  1:3  title  ’t  vs  ordered  denrh 


(elevation  angle) 


using  2:2 
using  1:3 


vs  z  (depth) 

vs  w  (heave) 

•t  vs  delta_planes 
’t  vs  ordered  depth 


using  2:8  title  ’t  vs  psi  (azimuth:  z  axis)  [deg] 

using  2:20  title  ’t  vs  psi^dot  (azimuth  rate) 

using  2:14  title  ’c  vs  r  (yaw  rate) 

using  2.10  title  ’t  vs  v  / & v i 


using  2:21  title  "t  vs  delta_rudder 


(yaw  rate) 
( sway ) 


using  1:2 


title  "t  vs  ordered  heading 


e)  [deg] 

[deg/sec 

]•' 

5)  [deg] 

[deg/sec 

]•; 

[ft] 

• 

[ ft/sec] 

m 

[deg] 

[ft] 

[deg] 

\ 

[deg/sec]  • 

\ 

[deg/sec]  • 

\ 

[  ft/sec]  ’ 

\ 

[deg] 

\ 

[deg] 

wit 

with  steps 


using  2:6  title  ’t  vs  psi  (azimuth:  z  axis)  [deg] 
using  2:20  title  ’t  vs  psi_dot  (azimuth  rate)  [deg/ 
using  2:14  title  ’t  vs  r  (vaw  rate)  fHorf/ 
using  2:10  title  -t  vs  v  isway)  \  tl/ 
using  2:27  title  -t  vs  bow  lateral  thruster  volt 


[deg/sec] ' 
[deg/sec] ' 
[  ft /sec]' 
[volts]  ' 


using  2:28  title  -t  vs  stern  lateral  thruster  [volts]  •  with 

using  1:2  title  "t  vs  ordered  heading  [deg]  •  with  steps 

8 

using  2:5  title  -t  vs  z  (depth)  rf.i  .  .. 

using  2:11  title  -t  vs  w  (heave)  ft/secl-'  \ 

— iir 


using  2:9  title  *t  vs  u  (surge)  [ft/sec] 
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pause  0  "page  12* 

set  title  'NPS  AUV  telemetry  12'  14,. 8 

p:..::t  "mission  .output  .  telemetry  •  using  2:10  title  "t  vs  v  (sway)  {ft/sec]" 

pause  C  "page  13" 

set  title  "NPS  AUV  telemetry  13"  14,-8 

plot  "mission . output . telemetry "  using  2:11  title  "t  vs  w  (heave)  [ft/sec]" 

pause  0  "page  14" 

set  title  "NPS  AUV  telemetry  14"  14,. 8 

Y-.lot  "mission -output ,  telemetry"  using  2:12  title  "t  vs  p  (roll  rate)  [deg/sec)" 

pause  0  "page  IS" 

set  title  "NPS  AUV  telemetry  15"  14,. 8 

piot  "mission -output -telemetry"  using  2:13  title  "t  vs  q  (pitch  rate)  [deg/sec] " 

pause  0  "page  16" 

set  title  "NPS  AUV  telemetry  16"  14,. 8 

plot  "mission .output .telemetry "  using  2:14  title  "t  vs  r  (yaw  rate)  [deg/sec]" 

pause  0  "page  1*’" 

set  title  "NFS  AUV  telemetry  17"  14,. 8 


plot 

"mission. output .telemetry" 

using 

2:21 

title 

"t 

vs 

delta_ rudder 

bow 

[deg]  • 

ptause 

0  "page  18" 

se:  r  : 

cle  "N-J^S  AU\-^  teleiTtetry  18" 

14,  .8 

t  aOt 

•rr-ssion  .output  .telemetry" 

using 

2:22 

title 

•t 

vs 

delta^planes 

bow 

[deg]  • 

•’page  19" 

i  T  C  '  1 

:  1 T  *  N r P  AUV  t  e  1  emie t  ry  1 9  * 

14,  .8 

"mission . output  .  telemietry " 

using 

2:23 

title 

"t 

vs 

rpm_left  [ 

rpm] 

s  \ 

•  m :  £  c  i  on  .  ou  t pu  t .  t  e  1  erne  t  r y  • 

using 

2.-24 

title 

•t 

vs 

rpm_right  [ 

rpm] 

\ 

using 

1:4 

title 

•t 

vs 

rpm_ordered  [ 

rpm] 

•  with 

steps 

:^a  use 

0  "page  20" 

se:  t  i 

tie  "NPS  AUV  telemetry  20" 

14,-8 

plot 

•mission .output . telemetry" 

using 

2:25 

title 

•t 

vs 

bow  vertical  thruster 

[volts] • 

with 

steps, 

"m.ission .  output .  telemetry" 

using 

2:26 

title 

•t 

vs 

stern  vertical  thruster 

[volts] • 

with 

steps, 

•mission  .output .  telem.etry " 

using 

2:27 

title 

•t 

vs 

bow  latera 

il  thruster 

[volts] " 

with 

steps, 

•  mit  E  £  t  on .  u  t pu  t .  t  e  1  emie t  ry " 

using 

2:28 

title 

"t 

vs 

stern  lateral  thruster 

(volts] " 

with 

steps 

pause  0  "plotting  individual  encapsulated  postscript  files..." 

stv  "NPS  AUV  telemetry"  29,  .8 

set  terminal  postscript  eps  color  "Courier"  14 

f-a  use  C  "figure  1  AUv^_x_y  .  eps  " 
set  output  "AUV^x.^y-eps" 

set  xlabel  "East  ->  (y^world)  [ft]" 
set  ylabel  "North  ->  (x_world}  [ft]" 
set  title  "NPS  AUV  telemetry  1"  26, .8 

plot  "mission .output .telemetry"  using  4:3  title  "x  vs  y  (geographic  position  plot)" 

set  xlabel  "time  t  (seconds)" 
set  ylabel 

pause  0  "figure  2  AUV_t_x.eps" 
sec  output  • AUV_t_x . eps" 

set  title  "NPS  AUV  telemetry  2"  26, .8 

plot  "mission .output . telemetry"  using  2:3  title  "t  vs  x  [ft]  •,  \ 

•mission. output .telemetry"  using  2:15  title  "t  vs  x_dot  { ft/sec]* 

pause  0  "figure  3  AUV_t_y.eps" 
set  output  • AUV_t_y.eps" 

set  title  "NPS  AUV  telemetry  3"  26, .8 

plot  "mission .output -telemetry"  using  2:4  title  "t  vs  y  [ft]  ",  \ 

•mission .output .telemetry"  using  2:16  title  "t  vs  y_dot  [ ft/sec)" 

pause  0  "figure  4  AUV_t_z.eps" 
set  output  "AUV^t^z .eps" 

set  title  "NPS  AUV  telemetry  4"  26, .8 

plot  "mission. output .telemetry"  using  2:5  title  "t  vs  z  (depth)  (ft)  ",  \ 
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•ir.lssion. output .  telemetry “ 
•mission . output . telemetry" 
•mission .output .orders" 

pause  0  ‘figure  B  AUV_t_phi.eps" 
set  output  •AU\'_t_phi  .eps  ■ 

title  'NPS  AUV  telemetry  5" 
plot  •mission .output . telemetry" 
•mission . output . telemetry" 


using  2:17  title 
using  2:22  title 
using  1:3  title 


using  2:6  title 
using  2:16  title 


•  ni  1  s  £  i  C'n  .  ou  t  p>u  t  .  t  e  1  erne t  ry  “ 
•mi  £31  -  ir-'Utp’jt  .  t  elemet  ry " 
•mission . output . telemet  ry " 
•mission .output . telemet  ry “ 
•miss.  .;r. .  -utput  .orders" 


•t  vs  z_dot  (depth  rate)  [ft/sec]",  \ 

"t  vs  delta_planes  I  deg]  •,  \ 

•t  vs  ordered  depth  [ft]  •  with  steps 


pause  0  ‘figure  6  AUV_t_theta.eps" 
set  output  * Airv'_t_theta  .  eps  • 

set  title  "NFS  AlA’’  telemetry  6"  26,  .8 
plot  •mission. output .telemetry"  '  using  2:7  title 
“mission .output . telemetry "  using  2:19  title 

pause  C  "figure  7  Al7V_t_theta_al  1 . eps • 
set  output  “AUV_t_theta_all.eps" 

set  title  "NFS  ALT’/  telemetry  7"  26,. 8 
i:d  jt  "mission .output .  telemetry "  using  2:7  title 


't  vs  phi  (roll:  X  axis] 

vs  phi_dot  (roll  rate) 


't  vs  theta 


using  2:7  title 
using  2 : 19  title 
using  2:5  title 
using  2:11  title 
using  2:22  title 
using  1:3  title 


[deg] 

[deg/sec]  ' 


'c  vs  theta 


't  vs  z  (de 

■t  vs  w  (he 

't  vs  delta_planes 
't  vs  ordered  depth 


(depth ) 
(heave ) 


angle) 

[deg] 

.  \ 

rate) 

[deg/sec] " 

angle) 

[deg] 

\ 

rate) 

[deg/sec] ■ 

\ 

[fc] 

\ 

[ft/sec]  ", 

\ 

[  deg  ] 

\ 

[ft] 

wii 

pause  0  •figure  6  AUV_t_psi.eps" 
se:  output  '  A’J’v_t_psi  .  eps " 

set  t^tle  ‘NFS  ALV  telemetry  8" 
p : jt  -mission. output . telemet  ry " 
•TTission  .  cutput .  telemetry" 
•mil ssicn. output .  telemetry" 
•mission . output . telemetry* 
•mission .output . telemetry " 
■mission . output . orders " 


using  2:8  title 
using  2:20  title 
using  2:14  title 
using  2:10  title 
using  2:21  title 
using  1:2  title 


jse  C  "figure  9  Airv_t_iateral_thrusters.eps" 

:  output  " AU\^_t_lateral_thrusters . eps • 

:  title  'NFS  A\JV  telemetry  9"  26,. 8 
3t  •mission. output . telemetry  using  2:6 

•mission. output . telemetry •  using  2:20 

•mission -output . telemetry "  using  2:14 

•mission . output . telemetry"  using  2:10 

•mission .output . telemetry •  using  2:27 

S 

•mi ssion. output . telemetry"  using  2:28 


(azimuth:  z  axis)  [deg] 
(azimuth  rate)  [deg/sec] 

(yaw  rate)  [deg/sec] 

(sway)  [  ft/sec] 

dder  [deg] 

heading  [deg] 


"t  vs  psi  (azimuth:  z  a> 

"t  vs  psi_dot  (azimuth  rate) 

•t  vs  r  (yaw  rate) 

*t  vs  V  (sway) 

■t  vs  delta_rudder 
't  vs  ordered  heading 


with  steps 


"t  vs  psi  (azimuth:  z  axis)  [deg]  ",  \ 

"t  vs  psi_dot  (azimuth  rate)  [deg/sec]",  \ 

vs  r  (yaw  rate)  [deg/sec]",  \ 

•t  vs  V  (sway)  [  ft/sec]",  \ 

"t  vs  bow  lateral  thruster  [volts]  "  with 


:  ssion . output . orders  * 


using  1:2 


•t  vs  stern  lateral  thruster  [volts] 

"t  vs  ordered  heading  [deg] 


raise  j  "figure  1C  AUV_t_vertical_thrusters.eps" 
set  output  “AUV_t_vertical_thrusters.eps" 

set  title  "NFS  AUV  telemetry  10"  26,. 8 
p'lot  “mission  .output .  telemetry "  using  2:5  title 

•mission .output  -  telemetry “  using  2:11  title 

•mission. output .telemetry  using  2:25  title 

•mission. output .telemetry"  using  2:26  title 

•mission-output .orders"  using  1:3  title 


•  with  steps 


•t  vs  z  (depth)  [ft]  •,  \ 

"t  vs  w  (heave)  [ft/sec]",  \ 

"t  vs  bow  vertical  thruster  [volts]  -  with  steps,  \ 

't  vs  stern  vertical  thruster  [volts]  •  with  steps,  \ 

■t  vs  ordered  depth  [ft]  "with  steps 


pause  0  “figure  11  AUV_t_u.eps" 
set  output  • AUV_t_u.eps" 

set  title  'NFS  AUV  telemetry  11"  26,. 8 
plot  "mission .output . telemetry 


using  2:9  title  "t  vs  u  (surge)  [ft/sec]' 


pause  0  “figure  12  AUV_t_v.eps" 
set  output  'AUV^t^v. eps" 

sec  title  “NFS  AUV  telemetry  12"  26,. 8 

plot  "mission. output .telemetry"  using  2:10  title  "t  vs  v  (sway)  [ft/sec]" 

pause  0  "figure  13  AUV_t_w.eps- 
set  output  ■AUV_t_w.eps" 

set  title  -NFS  AUV  telemetry  13"  26,. 8 

plot  "mission. output . telemetry  using  2:11  title  “t  vs  w  (heave)  [ft/sec] • 

pause  0  "figure  14  AUV_t_p.eps" 
sec  output  "AUV_t_j:).eps" 
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set  title  'NPS  AUV  telemetry  14*  26,. 8 

piot  'mission -output  -  telemetry "  using  2:12  title  't  vs  p  (roll  rate)  [deg/sec]' 

pause  0  "figure  15  AUV_t_q.eps' 
se*:  output  ' AU\^_t_q.eps" 

-ri-r'  ':tie  'Nr:-'  AUV  telemetry  15*  26,-8 

plot  'misston .output . telemetry '  using  2:13  title  't  vs  q  (pitch  rate)  [deg/sec] " 

pause  0  'figure  16  AUV_t_r-eps' 
set  output  •AUV_t_r  .ep>s' 

set  title  'NFS  AUV  telemetry  16'  26, .8 

plot  'mission . output . telemetry '  using  2:14  title  't  vs  r  (yaw  rate)  [deg/sec]' 

pause  C  'figure  17  AUV_t_delta_rudder.eps' 
set  output  •AUV_t_delta_rudder.eps' 

seT  title  'NP?  AUV  telemetry  17'  26,-8 

plot  'mi ssion . output . telemetry '  using  2:21  title  't  vs  delta_rudder  bow  [deg]' 

p^ause  0  'figure  16  AUV_t_delta_planes.eps' 
set  output  "A(JV_t_delta_planes -eps' 

set  title  'NFS  AUV  telemetry  16"  26, -8 

plot  'mission. output -telemetry  using  2:22  title  't  vs  delta_planes  bow  [deg]' 

pause  0  'figure  19  AUV_t_rpm.eps' 
set  output  •AUV_t_rprri- eps ' 

set  title  'NPS  AUV  telemetry  19'  26,-8 

plot  "mission. output -telemetry  using  2:23  title  't  vs  rpm_left  [rpm]',  \ 

•mission .output . telemetry '  using  2:24  title  't  vs  rpm^right  [rpm]',  \ 

'mission. output .orders'  using  1:4  title  't  vs  rpm_ordered  [rpm]'  with  steps 

pausT  t  'figure  2C  AUV_t_thrusters.eps' 
ser  lutpu:  •AUV_t_thrusters.eps' 

se*:  title  'NPF  AUV'  telemetry  20'  26,. 8 

'mission. output  .telemetry  using  2:25  title  "t  vs  bow  vertical  thruster  [volts]'  with  steps,  \ 

•m.i  ssion .  output .  telemetry  using  2:26  title  't  vs  stern  vertical  thruster  [volts]'  with  steps,  \ 

'mission. output -telemetry  using  2:27  title  't  vs  bow  lateral  thruster  [volts]'  with  steps,  \ 

•mission .output -telemetry  using  2:28  title  't  vs  stern  lateral  thruster  [volts]'  with  steps 

C  'gnuii.'-t  duv_iilot . gnu  complete.' 


« 
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IV.  UNDERWATER  VIRTUAL  WORLD  GRAPHICS 
A.  Introduction 

The  Open  Inventor  2.0  applications  program  interface  (API)  is  used  for 
generating  3D  computer  graphics  images  representing  the  virtual  world 
(Wemecke  94a,  94b).  Open  Inventor  was  chosen  for  the  virtual  world  graphics  engine 
due  to  its  being  built  upon  the  Open  GL  graphics  language,  expected  portability  to 
multiple  hardware  architectures  and  operating  systems,  SGI-standard  scene  description 
language,  object-oriented  design,  broad  flexibility  and  strong  support.  The 
Open  Inventor  scene  description  language  (.iv)  is  likely  to  become  a  compatible 
standard  with  numerous  graphics  engines  and  the  nascent  Virtual  Reality  Modeling 
Language  (.vrm/),  a  research  extension  to  Hypertext  Markup  Language  {.htmt). 

Embedded  in  the  graphics  viewer  code  is  a  Distributed  Interactive  Simulation 
(DIS)  2.0.3  multicast  network  API  which  permits  viewer  to  receive  robot  update 
information  nearly  anywhere  the  Internet  via  the  Multicast  Backbone  (MBone).  After 
the  configuration  file  .sd.tcl  is  installed,  the  viewer  program  can  be  launched 
automatically  via  the  MBone  session  directory  (sd)  advertisement  application,  enabling 
simple  remote  viewer  execution  and  connection  to  an  already-running  virtual  world. 
Future  work  includes  adding  a  WWW-compatible  scene  and  object  loader,  and  the 
capability  to  receive  URLs  for  retrieval,  either  via  DIS  updates  or  from  objects 
encountered  by  the  robot  that  are  already  in  the  scene  graph. 
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B. 


viewer.C  Object-Oriented  Real-Time  3D  Computer  Graphics  Viewing 
Program  using  Open  Inventor  2.0 


i  !  n ,!  i  !  I !  i  t !  I !  I !  I !  H  !  i  !  I !  I !  I  !  I !  I !  I !  I !  I !  I !  I !  I !  I !  I !  I !  I !  I !  I !  I !  I !  I !  I !  I !  I !  I !  I !  I !  I !  I ! 
!  * 

Program:  viewer  .C 

Description:  Open  Inventor  viewer  for 

NPS  AUV  Underwater  Virtual  World 

Autnor:  Don  Brutzman 

Revised :  28  October  94 

System:  Irix  5.2 

Compiler:  ANSI  C++  /  Open  Inventor  API 

Compilation:  irix>  make  viewer 

References:  (1)  IEEE  Protocols  for  Distributed  Interactive  Simulation  (DIS) 
Applications  version  2.0,  Institute  for  Simulation  and 
Training,  Universit  of  Central  Florida,  Orlando  Florida, 

26  May  1993. 

{2}  Macedonia,  Michael,  Zeswitz,  Steven,  and  Locke,  John, 
Distributed  Interactive  Simulation  (DIS)  multicast 
version  2.0.3,  Naval  Postgraduate  School,  February  94. 

(3)  Zeswitz,  Steven,  "NPSNET:  Integration  of  Distributed 
Interactive  Simulation  (DIS)  Protocol  for  Communication 
Architecture  and  Information  Interchange."  master's  thesis, 
Naval  Postgraduate  School,  Monterey  California,  28  May  1993. 

(4)  Wernecke,  Josie  and  the  Open  Inventor  Architecture  Group, 

_The  Inventor  Mentor_,  Addison-Wesley ,  Reading  Massachusetts, 
1994  . 

Advrsors;  Dr.  Mike  Zyda,  Dr.  Bob  McGhee  and  Dr.  Tony  Healey 

Notes:  Underwater  virtual  world  is  in  feet,  standard  DIS  PDU  is  meters 

Inventor  1.0.1  and  DIS  libraries  were  NOT  compatible  due  to 
programs  hangups  which  are  triggered  by  mallocs  in  the 
DIS  libraries  conflicting  with  the  Inventor  window. 

Removing  DIS  libraries  and  just  putting  malloc's  in  the 
Inventor  screen  redraw  callback  function  caused  identical 
program  hangups.  No  fix  was  found.  Upgrading  to  ppeninventor 
under  Irix  5.2  eliminated  this  problem  completely. 

Future  work:  Automated  camera  control 

.iv  file  load  via  WWW  URLs 
optimization 


//////////////////////////////////////////////////////////////////////////////// 

#include  • . . /dynamics/AUVglobals . H" 

# include  -riostieam . h> 
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^include  <iomanip.h>  //  must  follow  iostream.h 
^include  <scring.h> 

#include  <Tnath.h> 

^^include  <LimG.h> 

^include  '-get  opt.  h> 

^include  <stdlib,h> 

^include  <netdb.h> 
tinclude  <netinet/in . h> 

^include  <sys/types .h> 

i  !  I !  i  !  I  n  I !  n  !  I !  I !  I !  I  !  I !  I  !  I  n  !  I  !  I !  I !  I  n  I !  I !  I !  I !  I !  I !  I !  n  !  I II I !  I !  I !  I !  I !  I !  I !  I !  I !  I !  ! 

1 1  DIS  includes.  See  Makefile  for  ocher  DIS  #include  files;  they  must  match. 

<»i:'jciude  - /DIS  .mcast/h/disdef  s  .  h" 

^include  " . . /DIS . me as t /h/ appearance . h " 

:!  Old  DIS  include  statements  follow,  disregard... 

/•■■  tinclude  ' /n/dude/work /bruczman/DIE  .mcast/h/disdef  s .  h" 

.  /  *incl ude  " /n/ dude, 'work /brutzman/DIS . mcast /h/ appearance . h “ 

-■  /  Jc.hr.  Locke's  broadcast  version 

/•■  # include  ■  /n /gravy  1 /work /simnet /DIE- 2 . 0/h/disdef  s  .  h“ 

•■/  #inciude  ’ /n/gravy'l /work/simnec/DIS-2 . 0/h/appearance.h" 

Ma Cede n: -a / Zeswl t z  versions 

^ : :v: . u d-  “ / n / e i s i e / wor k 3 /ma cedon i/net /mcast / ne twor k / h/ di sde f s . h " 

.  ••  ^include  “ /n/elsie/work3 /macedoni /net/mcast /network/h/appearance  .h  “ 

.•  '  ^inciuoe  • /n/elsle/wcrkS/macedoni/net/mcast/network. round/h/disdef s .h" 

.'  '  #inc-Lude  ”  /n/eisie/work3  /macedoni  /  net/mcast  /network .  round/h/ appearance  .  h " 

extern  "C**  i  printPDU  (char  *);  };  //  function  prototype  provided  for 

//  compatibility,  missing  from  DIS  library 

.  ''///// //////////// //////////////////////////////////////////////// ///////////// 

^include  <X11 /Intrinsic .h> 

^include  <Xirt/Xirt . h> 

# include  <Xm/CascadeBG . h> 

^include  <-Xir/FcrTr .  h> 

* :  n c 2  u de  •  'Xrr  /  P owC c  1  umn  .  h 
« :  TiCl  ude  </ji/  PusnB .  h> 

^include  <Xm/PushBG .h> 

*  include  <Xm/Separa tor . h> 
include  <Xm/SeparatoG  .  h> 

*  include  -Xm/ToggleB . h> 

^include  <Xm/ToggleBG . h> 

#include  --Inventor/So .  h> 

*  include  <'IriVentor/SoDE  .  h> 

^include  «rlnventor/Xt /SoXt . h> 

tinclude  <Inventor/Xt /SoXtPrintDialog .h>  //  see  SoOf f screenRender 
^include  <Inventor/Xt /SoXtRenderArea . h> 

# include  <I nvent or /Xt /viewers /SoXtExami nerViewer .h> 

# include  <Inventor/sensors/SoTimerSensor . h> 

* include  <lnvencor/actions/SoCallbackAction .h> 

# include  <Inventor/actions/SoWriteAction.h> 

# include  <Inventor/engines/SoCalculator .h> 

#include  <Inventor/engines/SoElapsedTime . h> 

♦include  <Inventor/engines/SoTimeCounter . h> 

♦include  <Inventor/nodes/SoComplexi ty .h> 
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#include 
*  include 
# include 
# include 
« include 
# include 
^include 
# include 
include 
tinciude 
#include 
^include 
^include 
#include 
^include 
include 
<^incl  ude 
4»include 
^♦include 
# include 
#include 
#include 
in  nc^  ude 


<Inventor/nodes/SoCone .h> 
<Inventor/nodes/SoCoordinate3 . h> 
<InvenLor/nodes/SoCube .h> 
<Inventor/nodes/SoCylinder .h> 
<Inventor/nodes/SoDirectionalLight .h> 
^Inventor /nodes/SoDrawStyle . h> 
<'lnventor/nodes/SoFaceSec .  h> 

<Inventor /nodes/SoGroup . h> 
<Iriventor/nodes/SoMaterial  .h> 

-  Inventor /nodes /SoNurbsSur face .h> 

<1  nven  tor /nodes/ SoPerspect  i  veC  ainera  .h> 

< Invent or /nodes / SoPi ckS ty 1 e. h> 

<Inven tor /nodes /SoRotationXYZ.h> 
<Inventor/nodes/SoScal€, h> 
•'Inventor/nodes/SoSelection .  h> 

<I nven tor/ nodes /SoSepa rat or . h> 
<Inventor/nodes/SoSphere .h> 

< Inven tor/ nodes / SoSwi t ch .h> 

< I nven tor/ nodes /SoTransform.h> 
•'•Inventor/nodes/SoTransf  ormSeparator  .h> 
v'lnventor/nodes/SoTranslation.  h> 
<Iriventor/nodes/SoTriangleStripSet  .h>  // 
<Inventor/nodes/SoUnits . h> 


order  matters  here 


//  leftovers  from  conversion  1.0.1  =>  Openinventor 

//  ^include  <Inventor /Xt/SoXtColorEditor . h>  //  not  found! 

'■  /  i^include  <Inventor /nodes/SoBase . h> 

//  itinclude  <Inventor /nodes/SoCal  IbackList . h> 

^  ^include  <Inventor/nodes/SoInteraction  . h> 

//  ^include  <Inventor /Xt/SoXtFlyViewer . h> 

//  tinclude  <Inventor/Xt/SoXtPlaneViewer . h> 

//  ^include  <Inventor /Xt/SoXtWalkViewer .h> 


!  ‘ !  a  f !!!!  ^ !  I  i !  a  f  i  f !  n i i !  i  HI  I ! i  ( n  f  n  I  (!  I !!  f ! I  f  i  f ! I n n  ( u ! I ! I  f  n n /!  n ! I /  f  f ! / 
‘  i  function  prototypes 

0:311  i  n  t  D I  S^n e: _ope]*i  ( )  ; 

static  void  DI£_net_close  (); 

static  void  DI£_Pedraw_CaIlback  (  void  *  unused^data, 

SoSensor  *  unused_calling_sensor  ) ; 


//////////////////////////////////////////////////////////////////////////////// 

^define  PI  3.1415926535897932 

^define  METER£_PER_FT  0.3048 
^define  FT_PER_METERS  3.2808 

//  utility  function  prototypes 


double 

sign 

(double 

X)  ; 

double 

degrees 

(double 

x) ;  //  radians 

input 

double 

radians 

(double 

X);  //  degrees 

input 

double 

arcclamp 

(double 

X)  ; 

double 

dnormalize 

(double 

angle_radians) ; 

//  returns 

0.  .2PI 

int 

inormalize 

(double 

angle_radians) 

//  returns 

0.  .359 

(return 

(int)  (degrees 

(angle_radians)  +  0 
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SoSeparator  *  readFile ( const  char  *filenaite) ;  //  Inventor  Mentor  p.  284 
void  initial! ze_gIobais  (); 

void  parse_cornmand_Iine_f lags  (int  argc,  char  **  argv)  ; 

////.'/////////////////////////////////////////////////////////////////////////// 
//  global  data  (referenced  in  callback  routines,  thus  defined  here) 

SoSeparator  *  root; 

SoTransfcnrt  *  AUV_posi  tion_node; 

//  initializers  needed  to  avoid  startup  segmentation  fault 

SoFctat ionXYZ  *  rotate_AUV_z  =  new  SoRotacionXYZ ; 

SoRorar ionXYZ  *  rotate_AUV_y  =  new  SoRotationXYZ; 

SoRotat lonXYZ  *  rotate_AUV_x  =  new  SoRotationXYZ; 


SoRotationXYZ 

* 

r o  t  a  t  e_AUV_bow_r u  dde  r s 

= 

new 

SoRotationXYZ ; 

SoRotationXYZ 

* 

r  0 1  a  t  e_AUV_s  t  e  r n_r u  dde  r s 

= 

new 

SoRotationXYZ; 

SoRctat ionXYZ 

« 

r  0 1  a  t  e_A  UV_b  o w_p lanes 

= 

new 

SoRotationXYZ ; 

SoRotationXYZ 

r  0 1  a  t  e_A  U\"_s  t  e  r  n_p  lanes 

= 

new 

SoRotationXYZ 

^ oC one 

■» 

thrusterWakeF/ 

new 

SoCone; 

SoCc-ne 

thrusterWakeAV 

= 

new 

SoCone; 

SoCone 

* 

thrusterWakeFH 

new 

SoCone; 

CO 

o 

n 

0 

thrusterWakeAK 

= 

new 

SoCone; 

SoYone 

conePropellerWakeStbd 

= 

new 

SoCone; 

SoCone 

* 

conePropellerWakePort 

= 

new 

SoCone; 

£  oSw: t  c  n 

* 

whichWakeFV 

new 

SoSwi tch; 

SoSwi ten 

* 

whichWakeAV 

= 

new 

SoSwi tch; 

ScSwitch 

whichWakeFH 

new 

SoSwi tch; 

£ oSwj.  t oh 

* 

whichWakeAH 

= 

new 

SoSwi tch ; 

SoTransferm 

topsideBow 

new 

SoTransferm; 

SoTransf  oDTi 

» 

topsideStern 

= 

new 

SoTransf orm; 

SoTransferm 

♦ 

bottoms! deBow 

= 

new 

SoTransferm; 

SoTransf  onr. 

* 

bottomsideStern 

= 

new 

SoTransform; 

SoTransf  0  nr; 

lef tsideBow 

= 

new 

SoTransform; 

SoTransf orm 

* 

lef tsideStern 

= 

new 

SoTransform; 

SoTransf  orrr, 

•* 

rightsideBow 

= 

new 

SoTransform; 

SoTransf  onr. 

» 

rightsideStern 

new 

SoTransform; 

SoTransf  orr. 

* 

xfConeSonarSTlOOO 

new 

SoTransform; 

SOvCone 

* 

coneSonarSTlOOO 

= 

new 

SoCone; 

SoMaterial  *  silver 

=  new  SoMaterial; 

SoMaterial  *  gold 

=  new  SoMaterial; 

SoMaterial  *  brass 

=  new  SoMaterial; 

SoMaterial  *  chrome 

=  new  SoMaterial; 

SoMaterial  *  npsblue 

=  new  SoMaterial; 

SoMaterial  *  seagreer 

i  =  new  SoMaterial; 

SoMaterial  *  darkgreen  =  new  SoMaterial; 

SoMaterial  *  sonar_ 

red  =  new  SoMaterial; 

#define  CAMERA_FREE  0 
#define  CAMERA_TO_AUV  1 
#define  CAMERA_FROM_AUV  2 

static  int  whichCamera  =  CAMERA_FREE;  //  default  camera 

static  int  PRINTDIALOG  =  TRUE; 

static  int  BACKGROUNDCOLORDIALOG  =  FALSE; 


static  int  AUTOCAMERACONTROL 


FALSE; 


n  to  to  Ui  to  to  to  to  to 


sracic  int  TEXTURE  =  FALSE; 

SoRcri- aspect  I  veCamera  *  Perspect  i  veCameraFree  =  new  SoPerspectiveCamera ; 

SoPerspect i veCaiTiera  *  Perspect iveCameraToAUV  =  new  SoPerspectiveCamera; 

ScPerspectiveCamera  *  Perspect iveCameraFromAUV  =  new  SoPerspectiveCamera; 

/ /■  SbVec3f  cameraToAUVPosit ion; 

beh  i  ndAW  Position; 
aheadOfAUVPosi tion; 

p  r  i  o  r  AITV'  Posit!  on ; 
currentAUVPcsi tion; 

bVecjf  priorCameraPosi tion; 
bVec3f  currentCameraPosi tion; 
bVec3f  priorCameraOf f set ; 
oVecBf  currentCameraOf f set ; 

bV ec  3  f  o r i en t a  t i onPc t a  t i onAx i s ; 
loat  orientationRotation Angle ; 

"bVe-?:  St andardCameraCf f set  =  SbVec3f  (  -  20,0,  0.0,  2.0); 

float  standardCameraFocalDistance  =  20.0; 


SoSeparator  *  coimT'.and_l ine_node ; 

static  ciocK_t  tiiTie_stamp_of_current_PDU, 

time_of_PDU_receipt, 
current_clock., 
del ta_ciock; 

static  double  delta^time; 

star:--:  inr  PDU_overdue; 

static  irt  DIS_port„open ; 

rbar  port  {  6]; 

char  Qtoup  [30j; 

static  SoUnits  *  unitsfeet; 

static  struct  in^addr  inaddr; 

static  struct  hostent  *  hp; 


I  n  I !  i  I  n  n  n  n !/ N  n  I !  n !  I N 1 1  n !  i  n  n  n  II !  m  f  ^  ^  f  f  ^  ^  ^  ^  ^  f  ^  f  ^  ^  ^  ^  f  ^  ^  ^  f  ^  ^  ^  ^  ^  ^  ^  ^  ^  ^  ^ 

I I  all  NPS  AUV  dimensions  here  in  inches 

#define  HULLBODYLENGTH  54.00 
#define  HULLBODYWIDTH  16.50 
^define  HULLBODYHEIGHT  10.0 

#define  SEAM  -27.00 

tdefine  STERN  -43.50 

#define  LEFT  8.25 

#define  RIGHT  -8.25 

#define  TOP  5.00 

#define  BOTTOM  -5.00 


//  -  HULLBODYLENGTH  /  2.0 

//  -  HULLBODYLENGTH  /  2.0  -  16.5 

//  HULLBODYWIDTH  /  2.0 

//  -  HULLBODYWIDTH  /  2.0 

//  HULLBODYHEIGHT  /  2.0 

//  -  HULLBODYHEIGHT  /  2.0 


bVec  3  f 
b\V:3f 
bV  e  c  3 1 


♦define  DOMECONTROLPT  25.00 

♦define  DOMECONTROLPTHALF  20.00 


//  nosecone  NURBS  surface  cooordinates 
//  offsets  to  help  define  shape 


TOrHA^F  4.7  5 

4?def:ne  BOTTOMKALF  -4.75 

^define  LEFT_HALF  8.00 

♦define  KIGHTRhLF  -8.00 

♦define  CENTER  0.00 

♦define  FINLENGTH  6.00  //  fins  need  to  be  tapered 

♦define  FINWIDTH  0.75  //  fin  thickness  1*  at  bottoit/  0.5*  at  top 

♦define  FINHEIGHT  6.75 

♦define  FINOFFSETFORWARD  24.0 
♦define  FINOFFSETAFT  -33.0 

♦define  FINOFFSETLEFT  12.10  //  (HULLBODYWIDTH  /  2)  +  (FINHEIGHT  /  2)  +  .5" 

♦define  FINOFFSETRIGHT  -12.10 

♦define  FINOFFSETUP  8.875  //  (HULLBODYHEIGHT  /  2}  +  (FINHEIGHT  /  2)  e  .5- 

-  ♦define  FINOFFSETDOW^:  -8.875 

♦define  THRUSTER ID  3.0 

♦  oeiine  THRUSTEROEj  3.5 

♦define  THRUSTERFORWARDV  13.0  //  forward  SEAM  -  14 

♦  define  THRUSTE.RAFTV  -  21.0  //  SEAM  6 

♦define  THRUSTER FOR WARDH  19.0  //  forward  SEAM  -  8 

♦define  THRUSTERAFTH  -  26.0  //  SEAM  +  1 

♦define  SKAFTOFFSETLEFT  3.75 

♦define  SHAFTOFFSETRIGHT  -3.75 

/  /  reverify  tnese  dimensions  after  rebuild 
♦define  CARDCAGELEFT  4.00 

♦define  CARDCAGERIGHT  -4.00 

♦  detintr  CARvDCAGELENGTH  12.00 

♦define  CARDCAGEWIDTH  7 . 00 

♦  def^Tje  CARDCAGEHEI GHT  6.00 


//  profiling  sonar,  1  decree  conical 
/;  wacih  cut  feet  ->  inches  in  code 


♦define 

AUV_S71000  X  offset 

34.5 

// 

forward 

SEAM  + 

7.5“ 

♦define 

AUV  STIOOO^  offset 

-2.00 

// 

♦define 

AUTv^^STlOOO^z  offset 

4.00 

// 

bottom  : 

forward 

//  scanning  sonar  sector,  1 

degree 

wide  by  24 

degree 

vertical 

♦define 

Atr^/_ST725  X  offset 

31.5 

// 

forward 

SEAM  + 

4.5“ 

♦define 

Airv_ST7  2  5_:/_offset 

-2.00 

// 

♦define 

AL^/_5T7  2  5_z_cf  f  set 

-4 .00 

// 

top  aft 

/////// ////// ////// //////////////////////////////////////////////// ///////////// 
SoSepcrat or  *  makeA’J'^/  () 

rotate_AU\^_2->angle .  setValue  (  AUV_psi); 
rctate_AUV_2->  axis . setValue  (SoRotationXYZ ; :Z); 

rotate_AUV_y->anglG . setValue  (-  AUV_theta); 
rotat€_AUV_y->  axis . setValue  (SoRotationXYZ: :Y) ; 

rotate_AUV_x->angle. setValue  (  AUV_phi} ; 
rotate_AUV_x->  axis . setValue  (SoRotationXYZ: :X) ; 

SoDrawStyle  *wires; 
wires  =  new  SoDrawStyle; 
wires->style  =  SoDrawStyle :: LINES; 

//////////////////////////////////////////////////////////////////////////////// 
//  NPS  AUV  hull  body  center  is  at  [0.0  0.0  0.0], 

//  volumetric  center  of  buoyancy 

//  fin  transformations  forward 
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SoTransform  »xfl  =  new  SoTransform; 

xf i-vtranslatiori,setValue(  FINOFFSETFORWARD,  0.0,  FINOFFSETUP)  ; 
SoTransform  *xf2  =  new  SoTransform; 

xf 2>>translat ion. sec Value (  0.0,  0,0,  FINOFFSETDOWN  -  FINOFFSETUP); 
ScTransfcrm  *xf3  =  new  SoTransform; 

xf3--translation.setValue(  FIN OFFS ETFORWARD,  FINOFFSETLEFT,  0.0); 
SoTransform  *xf4  =  new  SoTransform; 

xf4>>translation.setValue(  0.0,  FINOFFSETRIGHT  •  FINOFFSETLEFT,  0.0) 
//  fin  transfoimations  afc 
SoTransform  *xf5  =  new  SoTransform; 

xf5->translation.setValue(  FINOFFSETAFT,  0.0,  FINOFFSETUP); 
SoTransform  *xt6  =  new  SoTransform; 

xf6->translation.secValue{  0.0  ,  0.0,  FINOFFSETDOWN  -  FINOFFSETUP); 
SoTransform  *xf7  =  new  SoTransform; 

xf"'->translation.setValuel  FINOFFSETAFT,  FINOFFSETLEFT,  0.0); 
SoTransform  *xf6  =  new  SoTransform; 

xfe-->cranslation.setValue(  0.0,  FINOFFSETRIGHT  -  FINOFFSETLEFT,  0.0) 

//  90  degree  increment  rotations  -  get  #define'd  PI  values 

SoRctat icnXYZ  *rc90x  =  new  SoRotat ionXYZ ; 
ro90x-‘>angle .  setValue  (3.141592653  /  2.0); 
ro90x->  axis . setValue  (SoRotat ionXYZ : :X) ; 

ScRotationXYZ  *ro90y  =  new  SoRotat ionXYZ ; 
ro90y- wangle . setValue  (3.141592653  /  2.0); 
ro9:'y->  axis  .setValue  (SoRotatioriXYZ  :  :  Y)  ; 

SoRotationXYZ  *ro902  =  new  SoRotationXYZ ; 
ro"r»'‘'z-'-angIe. setValue  (3.141592653  /  2.0); 
rc9  .  r-  .-  axis  .  setValue  (SoRotat  ionXYZ ::  Z)  ; 

SoRotationXYZ  *rol80x  =  new  SoRotationXYZ; 
rcl&ox-vangle. setValue  (3.141592653) ; 
rolBCx-'-  axis  .  setValue  (SoRotationXYZ  :  :X)  ; 

at:  onX Y  Z  r c  1  5  Oy  =  new  SoR o t a  t i  onXY Z  ; 
iv-  -angle. setValue  (3.141592653); 

axi s ,  setValue  (SoRotationXYZ :  :Y)  ; 

SoRotationXYZ  *rol802  =  new  SoRotationXYZ; 
roiS02->angle. setValue  (3,141592  653)  ; 
rcl802->  axis . setValue  {SoRotationXYZ : :Z) ; 

SoRotationXYZ  *ro270x  =  new  SoRotationXYZ; 
ro270x->angie. setValue  (-  3.141592653  /  2.0); 
rc270x->  axis. setValue  (SoRotationXYZ : :X) ; 

SoRotationXYZ  *ro270y  =  new  SoRotationXYZ; 
ro270y~>angle. setValue  (-  3.141592653  /  2.0); 
ro270y->  axis . setValue  (SoRotationXYZ : :Y) ; 

SoRotationXYZ  *ro2702  =  new  SoRotationXYZ; 
ro2702->angle . setValue  (-  3.141592653  /  2.0); 
ro2702->  axis. setValue  (SoRotationXYZ :: Z ) ; 

//  construct  NPS  AUV  hull  body 
SoCube  *hull  =  new  SoCube; 
hull->width  =  HULLBODYLENGTH; 
hull->height  =  HULLBODYWIDTH; 
hull->depth  HULLBODYHEIGHT; 

//  construct  a  control  fin 
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SoCube  *fin  =  new  SoCube; 
f i n - >w i d  r  h  =  FI NLENGTH ; 
f  in-'>heiaht:  =  FINWIDTK; 
fir:->depth  =  FINHEIGHT; 

-ronstruct  fin  pairs 

rotate_AW_bow_rudders  =  new  SoRotationXYZ; 
rot5te_AUV_bow_rudders->axis . setValue  (SoRotationXYZ: :Z) ; 
rotate_AUV_bow_rudders->angle. setValue  (  0.3  ); 

rotate_AUV_stern_rudders  =  new  SoRotationXYZ; 
rotate_AUV_stern_rudders->axis . setValue  (SoRotationXYZ: :Z) ; 
rotate_AlJV_stern_rudders->angle. setValue  (  0.6  ); 

rotate_AUV_bow_planes  =  new  SoRotationXYZ; 
rotate_AUV_bow_pIanes->axis . setValue  (SoRotationXYZ: :Z); 
rotate_AlTV_bow_planes- >angle . setValue  (  0.9  ); 

rota te_AUV_stern_planes  =  new  SoRotationXYZ; 
rotate_ALW_sCern_pianes->axis . setValue  (SoRotationXYZ ::Z); 
rot  a  te_Ab"V_stern_p  lane  9  -  >angle  .  setValue  (  1.2  ); 

//  construct  forward  vertical  fins  (bow  rudders) 

ScSeparator  *fvfins  =  new  SoSeparator; 
fvf ins->addChiid (  xfl  ); 

fvfins-  -addChild  ^  rot  a  te_AW_bow_r  udders  }  ; 
fviins- -ad jCni id i  fin  ); 
fvf  tr;S->addChild  i  xf2  ); 

fv:::'s->addChiId{  roieOx  );  //  net  rotation  180 

fvf in3->addCbild  (  fin  ); 

//  construct  aft  vertical  fins  (stern  rudders) 

ScSeparator  ^avfins  =  new  SoSeparator; 
avf  ins- :-addChi Id  f  xfS  ); 

avf  ins---addChild  {  rota te_AUV_stern_rudders  )  ; 
avf ij'iS- ^addCi;i id  ;  fin  ); 
avf  ins- .-addCnild  {  xf6  }; 

avf :ns->addChiid {  roieOx  );  //  net  rotation  180 

avf ins->addChi Id (  fin  ); 

//  construct  forward  horizontal  fins  (bow  planes) 

SoSeparator  *fhfins  =  new  SoSeparator; 
f hf ins->addChild (  xf3  ); 

fhfins->addChild(  ro90x  );  //  net  rotation  90 

fhfins->addChild {  rotate_AUV_bow_planes  ); 
fhf ins->addChild (  fin  ); 
fhf iriS->addChild  (  ro270x  ); 
f hf ins->addChild (  xf4  ); 

fhf ins->addchild (  ro270x  );  //  net  rotation  270  (in  case  of  fin  asymmetry) 

f hf 1 ns->addChi Id (  fin  ); 

//  construct  aft  horizontal  fins  (stern  planes) 

SoSeparator  *ahfins  =  new  SoSeparator; 
ahf ins->addChild (  xf7  ); 

ahfins->addChild(  ro90x  );  //  net  rotation  90 

ahfins->addChild(  rotate_AUV_stern_planes  ); 

ahf ins->addChild (  fin  ); 

ahf ins->addChild (  ro270x  ); 

ahf ins->addChild (  xf8  ); 

ahfins->addChild(  ro270x  );  //  net  rotation  270  (in  case  of  fin  asymmetry) 

ahfins->addChild(  fin  ); 

//  construct  cylinders  to  represent  the  thrusters 
ScTransform  *xfl3  =  new  SoTransform; 

xf 13->translation.setValue(THRUSTERFORWARDV,  0,0,  0.0); 
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SoTransform  *xfl4  =  new  SoTransform; 
xfl4->translation.setValue(THRUSTERAFTV,  0.0,  0.0); 
SoTransfonr,  *xfl5  =  new  SoTransfonn; 

xfl5->transIation.secValue(THRUSTERFORWARDH,  0.0,  0.0); 
SoTransfonr.  *xfl6  =  new  SoTransfonn; 
xfl6--transiation-setValue(THRUSTERAFTH,  0.0,  0.0); 


SoCv’Iinder  *  cubeV  =  new  SoCylinder; 
tubeV->radius  =  THRUSTERID; 
tubeV->heighr  =  HULLBODYHEIGHT  +  0,2; 

SoCy' Under  *  tubeH  =  new  SoCylinder; 

tubeH- >radius  =  THRUSTERID; 

tubeH- >height  =  HULLBODYWIDTH  +  0.2; 

SoCcmpIexi ty  *  wakeCompj exi ty  =  new  SoComplexi ty ; 
wakeCompIexlcy-vvalue  =  0.2; 


//  still  inches 
topside&ow-> 
bot  c  on;si  deBow>  > 
boc  t  orr:sl  deBow-  > 


translation .setValue(  0,  TOPHALF  +  AUV_bow_vertical ,  0); 
translation. setValuel  0,  BOTTOMHALF  +  AUV_bow„vertical ,  0); 
rotation .setValue (SbVec3f  {  0.0,  1.0,  0.0  ),  M_PI  ); 


top‘=^ideStern->  translation . setValue  (  0,  TOPHALF  +  AUV_stern_vertical ,  0) 
bottc:r.s:deS:Grn->translation.setValue{  0,  BOTTOMHALF  +  AUV_stern_vertical ,  0 ) 
DOt  tcTr.s:deStern->  rotation .  setValue  {SbVec3f  (  0.0,  1.0,  0.0),  M_PI  ); 


lef tsideBow->  translation . setValue (  0,  LEFT_HALF  +  AUV_bow_lateral ,  0); 
lef t si deBow- >  rotation . setValue (SbVec3f  {  0.0,  1.0,  0.0  ),  M_PI  ); 

r:gntsideBow->  translation . setValue (  0,  RIGHTHALF  +  AUV_bow_lateral ,  0); 

leftside£tern->  translation . setValue (  0,  LEFT.HALF  +  AUV_stern_lateral ,  0); 
lef tsideStern->  rotation . setValue {SbVec3f  (  0.0,  1.0,  0.0  ),  M_PI  ); 

rightsideStern---'  t ranslation  . setValue  {  0,  RIGHTHALF  AUV_stern_lateral ,  0); 


thrusterWakerv  =  new  SoCcne;  //  global  for  callbacks 
thrusterWa<eFV->height  =  AUV’_bow_vertical  *  2.0; 

thrusterWakeFV->bottoinRadius  =  AU\^_bow_vertical  /  4.0; 
thrusterWakePv ->parts  =  SoCone: : SIDES; 


whichWakerV  =  new  SoSwitch; 
wnachWakeFV->addChild  (topsideBow) ; 
whachWckerV->addCniid  (bcttomsideBow; ; 
wr.ichWar:eFV-->whichChild  =  0;  //  default  topsideBow 


ooSeparatcr  *  thruster^^  =  new  ScSeparator; 
thrusterrv---addCb:  Id  (  xfl3  ); 
thrusterFV->addChi Id (  ro90x  ); 
thrusterFV->addChild (  tubeV  ); 
thrusterFV->addChild (  wires  ); 
thrustei'FV->addChiid(  wakeComplexi ty  ); 
thrusteiFv ->addChild {  seagreen  ); 
thrusterrv/->addChild(  whichWakeFV  ); 
thrusterFV->addChild(  thrusterWakeFV  ); 

thrusterWakeAV  =  new  SoCone;  //  global  for  callbacks 
thrusterWakeAV->height  =  AUV_stern_vertical  ♦  2.0; 

thrusterWakeAV->bottoinRadius  =  AUV_stern_vertical  /  4.0; 
thrust€rWakeAV->parts  =  SoCone :: SIDES; 

whichWakeAV  =  new  SoSwitch; 
whichWakeAV->addChild  (topsideStern) ; 
whichWakeAV->addChild  (bottomsideStern) ; 
whichWakeAV->whichChild  =0;  //  default  topsideStern 

SoSeparator  ♦  thrusterAV  =  new  SoSeparator; 
thrusterAV“>addChi Id (  xfl4  ); 
thrustei-AV->addChild  (  ro90x  ); 
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'nrusi:erAV->addChi Id  f  tubeV  ); 
rhruscerAV'>addChiId  (  wires  ); 
i:hruscerAV->addChiid  (  wakeCompIexi  ty  }; 
t:hrusterAV->addChi  Id  (  seagreen  }; 
chrus!:erAV->addChi  Id  (  whichWakeAV  ); 
ri.rusceiAV- --addChi  Id  (  thrusterWakeAV  ); 

thrusterWakeFH  -  new  SoCone;  //  global  for  callbacks 

thru3t:erWakeFH->height  =  AUV_bow_lateral  *  2.0; 

thrust:erWakeFH->bottomRadius  =  AUV_bow_lateraI  /  4.0; 

chrusterWakeFH->parts  =  SoCone: rSIDES; 

whichWakeFH  ==  new  SoSwicch; 
whichWakeFH->addChild  ( lef tsideBow) ; 
whichWakeFH->addChild  (rightsideBow) ; 
whichWakeFH->whichChild  =  0;  //  default  leftsideBow 

SoSeparator  *  thrusterFH  =  new  SoSeparator; 
thrusterFH->addChild :  xfl5  ); 
thrusterFH->addChild  1  ro90y  ); 
thrusterFH->addChi Id (  tubeH  ); 
tnrust erFH->addChi Id (  rolSOz  ); 
tnrusrerFK->addChi id (  wires  ); 
trjrusrerFH->addChiid  (  wake  Comp  lex  icy  ); 
thrusterFH”>addChi Id (  seagreen  ); 
chrusterFH->addChi Id (  whichWakeFH  }; 
thrust erFH->addChi Id (  thrusterWakeFH  ); 

thrusterWaKeAH  =  new’  SoCone;  //  global  for  callbacks 
thrusterWaKeAH->heaght  =  AUV_stern_i at eral  *  2.0; 

thrust erWaKeAH->bot tomRadius  =  Ab^\/_stern_lateral  /  4.0; 

trirusterWakeAH->parts  =  SoCone:  :SIDES; 

whichWakeAH  =  new  SoSwitch; 
whichWakeAH->addChiId  {lef tsideStern) ; 
whichWakeAH- ■>addCni Id  (rightsideStern)  ; 
whichWakeAH->whichChiId  =  0;  //  default  lef tsideStern 

SoSeparator  *  thrusterAH  =  new  SoSeparator; 
thruscerAH->addChi Id {  xfl6  ); 
thrusterAH->addChiId  (  ro90y  ) ; 
thrusterAH- -addChi Id (  tubeH  ); 
thrusterAH->addChiId (  rolSOz  ); 
tnrusterAH->addChi id (  wires  ); 
thrusterAH->addChi Id (  wakeComplexi ty  ); 
thrusterAH->addChild (  seagreen  ) ; 
thrusterAH->addChiId (  whichWakeAH  ); 
thrusterAH”>addChild (  thrusterWakeAH  ); 

//  construct  internal  CARDCAGEs  left  and  right 

SoCube  *cardcagelef tbox  =  new  SoCube; 
cardcagelef Cbox->width  =  CARDCAGELENGTH ; 
cardcagelef tbox->height  =  CARDCAGE WIDTH ; 
cardcagelef tbox->depth  =  CARDCAGEHEIGHT; 

SoTransform  *xf cardcagelef t  =  new  SoTransform; 

xf cardcagelef t->translation. setValue (  0.0,  CARDCAGELEFT,  0.0  ); 

SoSeparator  *cardcagelef t  ==  new  SoSeparator; 
cardcagelef t->addChi Id (  xf cardcagelef t  ); 
cardcagelef t->addChild {  cardcagelef tbox  ) ; 

SoCube  *cardcagerightbox  =  new  SoCube; 
cardcagerightbox->width  =  CARDCAGELENGTH; 
cardcagerightbox->height  =  CARDCAGEWIDTH; 
cardcagerightbox->depth  =  CARDCAGEHEIGHT; 
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SoTransf oriTi  *xf cardcageright  =  new  SoTransform; 

xf cardcagerighc->Lranslation . setValue {  0.0,  CARDCAGERIGHT,  0.0  ); 

SoSeparator  *cardcageright  =  new  SoSeparacor; 
cardcagerighi:->addChild(  xf cardcageright  ); 
cardcageright->addChiId (  cardcagerightbox  ) ; 


//////////////////////////////////////////////////////////////////////////////// 
//  construct  main  body  out  of  parts 
SoGroup  *body  =  new  SoGroup; 


body->addChild ( 
body->addChild ( 
boc^' ->addcbi  Id  ( 
bcd>'->addChi  Id  f 
bod\^->addChi  Id  ( 


gold  )  ; 
fvfins  )  ; 
avfins  )  ; 
fhfi ns  }  ; 
ahfins  )  ; 


body->addChi Id  i 
h.-ody-  -addChi  Id 

body--*addChi  Id  ( 
body  ~'*addCni  id  ? 
bod>'->addChl  Id  = 
bcd'y-  >addChi  id  { 
bcdy->addChi Id ( 
body->addChi Id ( 
bcdy->addChi Id ( 


npsbiue  ) ; 

-bull  ); 

si Iver  ; ; 
thrusterFV  } ; 
thrusterAV  ) ; 
thrusterFH  } ; 
thrusterAH  ); 
cardcageleft  )  ; 
cardcageright  )  ; 


//////////////////////////////////////////////////////////////////////////////// 
//  construct  nosecone  using  a  NURBS  surface 


static  float  pts  [25] [5]  = 
{ 


- 

SEAN , 

LEFT, 

TOP 

) 

_ 

SE.AM , 

LEFT_HALF, 

TOP 

) 

{ 

- 

SEAN, 

CENTER, 

TOP 

) 

i 

- 

SEAN, 

RIGHTHALF, 

TOP 

) 

{ 

- 

SEAN, 

RIGHT, 

TOP 

} 

{ 

SEAl^, 

LEFT, 

TOPHALF 

{ 

- 

SE.^bN  - 

DOMECONTP.OLPTHALF , 

LEFT^HALF, 

TOPHALF 

{ 

- 

SEAM 

DOMECONTPOLPTHALF , 

CENTER, 

TOPHALF 

{ 

SEAM  - 

DOMECONTROLPTHALF , 

RIGHTHALF, 

TOPHALF 

{ 

- 

SEA>U 

RIGHT, 

TOPHALF 

), 

{ 

SEAM, 

LEFT, 

CENTER 

), 

/ 

- 

SEAM  + 

DOMECONTROLPTHALF , 

LEFT.HALF, 

CENTER 

i 

_ 

SEAM  + 

DOMECONTROLPT, 

CENTER , 

CENTER 

{ 

- 

SEAi*^  + 

DOMECONTROLPTHALF , 

RIGHTHALF, 

CENTER 

{ 

- 

SEAM, 

RIGHT, 

CENTER 

{ 

SEAM, 

LEFT, 

BOTTOMHALF 

{ 

- 

SEAM  + 

DOMECONTROLPTHALF , 

LEFT.HALF, 

BOTTOMHALF 

{ 

SEAM  + 

DOMECONTROLPTHALF , 

CENTER, 

BOTTOMHALF 

), 

{ 

_ 

SEAM  + 

DOMECONTROLPTHALF , 

RIGHTHALF, 

BOTTOMHALF 

{ 

- 

SEAM, 

RIGHT, 

BOTTOMHALF 

{ 

SEAM, 

LEFT, 

BOTTOM 

{ 

_ 

SEAM, 

LEFT_HALF, 

BOTTOM 

{ 

_ 

SEAM, 

CENTER, 

BOTTOM 

}, 

{ 

_ 

SEAM, 

RIGHTHALF, 

BOTTOM 

{ 

- 

SEAM, 

RIGHT, 

BOTTOM 

}; 

static  float  knots  [10]  = 
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(  0,  0,  0,  0,  0,  1,  1,  1,  1,  1  }; 

SoCoinplexicy  *noseconeCoitip2exity  =  new  SoComplexitv 
no^econeCoinpIexi  cy-- -value  =  0.7; 


SoCoordinateS  -control Pts  =  new  SoCoordinate3; 
conti-olPts->point.setValues  (  0,  25,  pts  ); 

SoNurbsSurface  -sonardoine  =  new  SoNurbsSurface; 
Poriardorne->nuiiiUControi Points . setValue  (  5  ); 
sonardonie-'.numVControl Points . setValue  (  5  )■ 
sonardoine->uKnotVector.setValues  (  0,  10,  knots  )• 
sonardcme->vKnotVector . setValues  (  0,  10,  knots  ) ; 

SoSeparator  -nosesection  =  new  SoSeparator; 
nosesect j on- >addChi Id (  npsbiue  ); 
noseseccion->addChiid f  noseconeComplexi ty  )■ 
nosesection->addChild(  controlPts  ); 
ncsesect i on- >addChi Id (  sonardome  ) ; 


ccneSonarSTlOOO  =  new  SoCone • 
ccneSonarST:000->h€ight: 
ccneSonarSTl 000->bot comRadius 
c ones on a  rSTl 000- >parts 


//  global  for  callbacks 
=  fabs  (AlJ\"_ST1000_ranae } 
=  fabs  (AUV_ST1000_range) 
=  SoCone: : SIDES; 


/  60.0;  //  1  degree 


//  drawn  frorr.  center 

>::?one£cnar£T100(.->translation  .  set  Value  f  0.0, 

//  -  (HULLBODYLENGTH  /  2.0) 

-  {AW_ST1000_range/  2.0),  4.0/12.0); 

^-Cir^^paratc:'  sepSonar  =  new  SoSeparator; 
sepSonar- »addChl Id  '  rc90z  ); 
sepSofiar-  -aodCl.ild;  wires  )  ; 
sepSonar->adclChi Id  ■;  wakeCojnplexi  ty  ;; 
sepSonar->addChiId (  sonar_red  ;; 
sepSonar-^addChl Id i  xfConeSonarSTl 000  ); 
sepSonar-vaddChi Id f  coneSonarSTl 000  ) ' 


Define  tail  section  ^^'^^///'^///////////////////////// 


>' ■  ensure  polygons  are  defined  in  clockwise  fashion! 


//  Two  triangles  using  SoTriangl eStripSet : 
static  long  numbert rianglevertices  [2]  =  {3,  3); 


static  float  aft trianglevert ices 


[6]  [3]  - 


{STERN,  LEFT,  0. 
(STERN,  RIGHT,  0. 
} ; 


},  (SEAM,  LEFT, 
},  (SEAM,  RIGHT, 


TOP],  (SEAM, 
BOTTOM},  (SEAM, 


LEFT,  BOTTOM], 
RIGHT,  TOP), 


y  Define  coordinates  for  triangular  vertices  &  SoTriangleStripSet 
SoCoordrnateS  -tailcoordl  =  new  SoCoordinateS ;  ^  riptet 
tailcoordl ->point . setValues  (0,  6,  afttrianglevertices) ; 


SoTriangleStripSet  *tail triangleset  = 
tail  triangleset->nuinVert ices  .setValues 


new  SoTriangleStripSet ; 


(0,  2,  nujTibertrianglevertices)  ; 


//  Two  rectangles  using  FaceSet: 

static  long  numbergu advert ices  [2]  =  (4,  4);  //  ref.  p.  5-6 

static  float  af tquadvertices  (8] [3]  = 

(STERN,  LEFT,  0.0),  (STERN,  RIGHT,  0.0),  (SEAM,  RIGHT  TOP) 
(SEAM,  LEFT,  TOP],  '  i  > 
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(STERN,  RIGHT,  0.0),  (STERN,  LEFT,  0.0),  (SEAM,  LEFT,  BOTTOM), 

(SEAM,  RIGHT,  BOTTOM), 

); 

! !  Define  coordinates  for  quad  vertices  &  SoFaceSet 
SoCoordinate3  *tailcoord2  =  new  SoCoordinate3 ; 
taiiCoord2->pcint .setValues  (0,  8,  af tguadvertices ) ; 

SoFaceSet  *tailquadset  =  new  SoFaceSet; 

ta i I quadset->nuiTiVert ices  .setValues  (0,  2,  numberquadvertices ) ; 

//  Two  cylinders  currently  represent  the  propellers  -  improve  this! 

//  a  much  fancier  individual  prop  model  is  possible  here;  also  add  complexity 
SoCylinder  *prop  =  new  SoCylinder; 
prop->radius  =  2.00; 
prop->height  =  1.00; 

//  Two  cylinders  to  represent  the  shafts 
SoCylinder  ‘shaft  =  new  SoCylinder; 
shaf  !:->radius  =  0.50; 
shaf t->height  =4.00; 

SoTransform  *xfl8  =  new  ScTransform;  //  shafts  relative  to  props 

//  note:  rotated  90z 

x: 16->translation . setValue (0.0,  -2.0,  0.0;; 

SoTransform  ‘xfll  =  new  SoTransform;  //  left  prop 
xfi:‘>translation. setValue (STERN  -  2.0,  SHAFTOFFSETLEFT,  0.0); 

//  compose  shaft  with  prop 

SoSeparator  ‘leftprop  =  new  SoSeparator; 

lef tprop->addChi Id  (  xfll  ); 

lef tprop->addChi Id (  ro90z  }; 

ief tprop- >addChi Id (  prop  ;; 

lef tprop->addChi Id (  xfl8  ); 

lef tprGp->addChi Id (  shaft  ); 

SoTransform  ‘xf PropellerWakePort  =  new  SoTransform; 
xf FropeilerWakePort*>translation.setVaiue{  0.0,  15.0,  0.0  ); 

SoSeparator  *  separatorPropel lerWakePort  =  new  SoSeparator; 
separatorPropeIlerWakePort->addChild(  xf Propel lerWakePort  ); 
separatorPropellerWaKePort->addChild (  rol80x  ); 
separa torPropellerWakePort->addChi Id (  wires  ); 
separatcrPropellerWaxePort->addChild (  wakeComplexi ty  ); 
separatorPropel lerWakePort->addChi Id (  seagreen  ); 
ccnePropel lerWakePort  =  new  SoCone;  //  global  for  callbacks 
ccneFropellerWakePcrt->height  =  AUV_port_rpm  /  700.0  *  24.0; 

conePropel lerWakePort->bot tomRadius  =  fabs  (AW_port_rpm)  /  700.0  *  6.0; 
corjePropellerWakePort->parts  =  SoCone  ::  SIDES ; 

separatorPropellerWakePort->addChild (  conePropellerWakePort  ) ; 
lef tprop“>addChild (  separatorPropellerWakePort  ) ; 

SoTransform  ‘xfl2  =  new  SoTransform;  //  right  props 
xfi2->tran3lation. setValue (STERN  -  2.0,  SHAFTOFFSETRIGHT,  0.0); 

SoSeparator  *rightprop  =  new  SoSeparator; 
rightprop->addChild (  xfl2  ); 
rightprop- >addChild (  ro90z  ); 
rightprop->addChild (  prop  ); 
rightprop->addChild (  xfl8  ); 
rightprop->addChild (  shaft  ); 

SoTransform  *xf Propel lerWakeStbd  =  new  SoTransform; 
xf PropellerWakeStbd->translation.setValue(  0.0,  15.0,  0,0  ); 

SoSeparator  *  separatorPropellerWakeStbd  =  new  SoSeparator; 
separatorPropellerWakeStbd->addChild(  xf Propel lerWakeStbd  ); 
separatorPropellerWakeStbd->addChild(  rol80x  ); 
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separatorPropellerWakeStbd->addChild (  wires  ); 
SGparatorPropGllerWakeStbd->addChild {  wakeComplexi ty  ) ; 
separatorPrcpGllerWakeScbd->addChi Id {  seagreen  ); 
conePropel lerWakeStbd  =  new  SoCone;  //  global  for  callbacks 
conePropellerWakeStbd->height  =  AUV__port_rpiri  /  700.0  24.0; 

conePropel lerWakeStbd->bottomRadius  =  fabs  (AUVjiort^rpin)  /  700.0  *  6.0; 
conePropel lerWakeStbd->parts  =  SoCone: : SIDES; 

SGparatorPropellerWakGStbd->addChild f  conePropel lerWakeStbd  ) ; 
rightprop->addChild(  separatorPropel lerWakeStbd  ) ; 

//  construct  stern  box  support  beneath  aft  vertical  fins 

SoCube  *sternf insupportcube  =  new  SoCube; 

sternf insupportcube->width  =  FINLENGTH  +  1.5; 

sternf insupportcube->height  =  5.0; 

sternf insupportcube->depth  =  HULLBODYH EIGHT  -  0.5; 

SoTransfonn  *xfl7  =  new  SoTransform; 
xf 17->translation . setValue {FINOFFSETAFT,  0.0,  0.0); 

SoSeparator  ^sternf insupport  =  new  SoSeparator; 

sternf insupport>>addChild {  xfl7  ); 

sternf insupport->addChild (  sternf insupportcube  ) ; 

SoSeparator  tai Isecti on  =  new  SoSeparator; 
ta:lse:: :on->addChild(  npsblue  ); 
tailsect ion->addChild (  tailcoordl  ); 
taiisec: ion->addChild (  tai Itrianglesec  ) ; 
trail  s^o::on-''-addChild(  tail  coo  rd2  )  ; 
ta:lsection->addChild {  tailguadset  ); 
tailsection->addChild (  sternf insupport  ) ; 
tailsec: ion->addChild •  brass  ); 
ta- 1  sec:  .  on- .'cddChild  ■  leftprop  /; 
tallsect  ion-:^addChiid  ;  rightprop  }; 

/.•////.'  I!  !  !  !  n  !  i  !  !  i  i  i  i  i!  !  i  !  !  !  !  I  !  f  f  !  !  I !  !  I  f  !  !  i  i  n  !  I !  j  !  !  I  i  n  !  J  j  !  !  I !  I  i  !  I  if  f  j  j  i  ! 

robot  is  just  units  h  body  &  nosesection  h.  tailsection  &  extra  stuff 

SoSeparator  *  robot  =  new  SoSeparator; 
robct--^addChild  (  unitsfeet  ); 

//  robot  root  transform  for  overall  vehicle  orientation 
Airw'_pcs: tion_node  =  new  SoTransform; 

AlW_pos: t i on_node->transl a ti on .setValue (0.0,  0.0,  0.0); 
robct->addChlld (  AUV_posi tion_node  ); 

rota te_Al.TV_z  =  new  SoRotationXYZ ; 
rotate_AUV_2->angle . setValue  (  -  AUV_psi); 
rotate_AUV_2->  axis . setValue  (SoRotationXYZ: :Z) ; 
robot->addChild (  rotate_AUV_z  ); 

rotate_AUV_y  =  new  SoRotationXYZ; 
rotate_AUV_y->angle . setValue  (  -  AUV_theta); 
rotate_AUV_y->  axis . setValue  (SoRotationXYZ : :Y) ; 
robot- >addChi Id (  rotate_AUV_y  ); 

rotate_AUV_x  =  new  SoRotationXYZ; 
rotat€_AUV_x->angle. setValue  (  AUV_phi); 
rotate_AUV_X“>  axis . setValue  (SoRotationXYZ : :X) ; 
robot->addChild (  rotate_AUV_x  ); 

robot->addChild (  sepSonar  );  //  feet 

SoUnits  ^unitsinches  =  new  SoUnits; 

uni tsinches->uni ts . setValue  (  SoUnits :: INCHES  ); 

robot ->addChild (  unitsinches  ); 
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SoPlckStyle  *  unpi ckablestylenode; 

SoPickSryle  *  pi ckablestylenode; 
unpickablestylenode  =  new  SoPlckStyle; 

pickablestylenode  =  new  SoPlckStyle; 
unpickablestylenode->styl€-setValue  (  SoPlckStyle : rUNPICKABLE  ); 
pickablestylenode->style . setValue  (  SoPlckStyle : :BOUNDING_BOX  ); 

//  Make  subsequent  nodes  unpickable  so  that  AUV  is  treated  as  a  whole 
robot ->addChild (  unpickablestylenode  ); 

robot->addChild (  body  ); 
rcbcr-^^addChild  (  nosesection  }; 
robot->addChild (  tailsection  ); 

ScTransfonn  *xfl9  =  new  SoTransform; 

xf 19->translation. setValue  (  0.0,  0.0,  -  2.0); 

robot ->addChil d (  xfl9  ); 

rcbct-'*addChild  (  new  SoPointLlght  ); 

ccut  <-  “new  point  light  added  under  robot*  «  endl ; 


return  rcoot; 


/////'///////// ///////////////////////////////////////////////////////////////// 

douche  sign  (double  x; 

If  (X  >  0.0)  return  1.0; 

else  if  {X  <  0.0)  return  -1.0; 


else  return  1.0; 

} 

// - // 


double  degrees  (double  x)  //  radians  input 


// 


double  radians  (double  x)  //  degrees  input 
return  x  *  PI  /  180.0; 


- // 

double  arcclamp  (double  x) 
if  (X  >  1.0) 

X  =  1.0; 

cout  «  “viewer:  arcclainp  reduced  “  «  x  «  “  to  1.0"  «  endl; 

} 

else  if  (X  <  -1 . 0} 

{ 

X  =  -1.0; 

cout  «  "viewer:  arcclamp  raised  "  «  x  «  "  to  -1.0"  «  endl; 

) 

return  x; 

) 

//-- - - - - - - // 

double  dnormalize  (double  angle_radians ) 
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double  new_angle  =  angle^radian^ ; 

wh:2e  fnew_arigle  >  2*PI)  new_angle  ”=  2*PI; 
w::l:e  fnew^arigle  -r  0.0  )  new^angle  2* PI; 
return  new_angle; 

] 

//////////////////////////////////////////////////////////////////////////////// 


// 


// 


static  int  DIS_net_open  (}  //  Ref:  Macedonia  include  files 


''  '  Multicast  Defaults  from 

//  /n/el sie/work3/macedoni /net /mcast /network /utils /planes /planes . cc 

//  ttl  value  does  not  matter  since  this  viewing  program  only  reads  PDUs 

u_char  ttl  =  16;  //  multicast  ttl=16  stays  inside  NPS 

int  exercise_id  =  -1; 

int  coordinate_system  =0;  //  0  =  flat  world,  1  =  round  world 

char  *  utm_file  =  " ^ ; 

int  result  =  net_cpen  (port,  group,  ttl, 

exercise_id,  coordinate_system,  utm_file); 

^.nt  result  =  net^open  (port,  group,  ttl);  //  old  version 


result  ==  FALSE: 


cour  "viewer:  DIS_net_open  ()  failed"  «  endl  ; 
DlS^pcrt^open  =  FALSE; 

I 

el  se 


/  /  cout 
/  /  < 
if  cout 


"viewer:  port  =  "  <<  port 


ttl  =  "  <<  ttl 

« 

exercise_id  =  *' 

<< 

coordinate_sy stem  =  " 

<< 

u  tm_f i 1 e  =  \ " "  «  u  tm. 

.file 

«  ",  group  =  "  «  group 
endl  ; 

exercise_id 
coordinate_system 
«  • \ "  «  endl ; 


t ; 


// 


static  void  DIS_Redraw_Callback  (  void  *  unused^data, 

SoSensor  *  unused_cailing_sensor  ) 

\ 


double  delta_x 

= 

0.0; 

double  Qelza_y 

0.0; 

double  delta_z 

0.0; 

double  delta_phi 

0.0; 

double  delta_theta 

= 

0.0; 

double  delta_psi 

= 

0-0; 

int 

number_of_PDUs  ; 

Enti tylD 

UUV_DIS_id; 

Enti tyType 


UUV_DIS_type; 


Enti tyStatePDU 


*  UUV_DIS_pdu  =  NULL; 


char 


♦  local_PDU  =  NULL; 
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locai_PDU_cype; 


PDUT'ype 

static  int  rcvd  =  0; 

char  NPSAlTV_Marking  [8]; 

bzerc  (NP£AUV'_Marking ,  8); 

strncpy  (NPSAUV_Marking,  “NPS  AUV-,  7);  //  4  more  free  chars  remain 
if  (delta^time  <=  5.5} 

cout  <<  "viewer:  DIS_Redraw_Callback :  PDU  loop  delta_time  =  “ 

<<  delta_time  <<  endl ; 

while  (TRUE)  //  until  break 
{ 

number_of_PDUs  =  net^read  (&  local_PDU,  &  local_PDU_type) ;  //  old  version 

number_of_PDUs  =  net_read  {&  local_PDU,  &  local_PDU_type,  &  inaddr); 

//  cout  ♦'<  "viewer:  net^read  complete,  number_of_PDUs  available  = 
number_cf_PDUs  endl  ; 

if  ( n um be i _o f _PDU s  =  =  • 1 ) 

cout  "viewer:  Error  on  net_read,  number_of_PDUs  =  -1"  «  endl; 
if  ' number_of_FDUs  <=  0) 

break;  //  no  more  PDUs,  done  for  now 
//  rcvd 

//  cout  «  "PDU  received,  rcvd  =  "  «  rcvd  «  endl; 

/  /'  print  PDU  ( 1  o  c  a  1_PDU )  ; 

UUV'_DI£_pdu  =  (Enti  tyStatePDU  *)  local_PDU; 

//  ensure  the  PDU  values  are  the  right  types 
if  (iccal_PDU_type  !=  EntityStatePDU_Type) 

cout  «  "viewer:  local^PDU^type  !=  Enti tyStatePDU_Type,  ignored..." 

<<  end! ; 

/ /  don't  forget  this  or  get  a  memory  leak! 

//  articulated  parameters  are  also  freed 
freePDU  ((char  *)  UUV  PIS  pdu ) ; 

cout  "viewer:  freePDU  ((char  *)  UUV_DIS_pdu)  called  for  this  PDU" 

<<  endl ; 

continue;  //  not  a  break  since  other  PDUs  may  be  waiting 

} 

else  if  (strncmp  ((char  *)  UUV_DIS_jpdu->entity_marking. markings, 

NPSAUV^Marking,  7)  !=  0) 

^  cout  «  "viewer:  non-NPS  AUV  Entity  State  PDU  encountered,  ignored..." 
«  endl ; 

//  printPDU  (local^PDU) ; 
freePDU  ({char  *)  UUV_DIS_pdu ) ; 

//  don't  forget  this  or  get  a  memory  leak! 

//  articulated  parameters  are  also  freed 

cout  «  "viewer:  freePDU  {(char  *)  UUV_DIS_pdu)  called  for  this  PDU" 

«  endl ; 

continue;  //  not  a  break  since  other  PDUs  may  be  waiting 

) 

//  cout  «  "PDU  OK"  «  endl; 

//  extract  parameters  of  an  entity  state  PDU  (most  are  listed  in  pdu.h) 

It  this  assumes  there  are  no  articulated  parameters  (add  later)  «< 
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If  DIE  ID  and  Type 

//  lJUV_DI£_id  =  UUV_DIS_pdu->enti  ty_id; 
if  UUV_DI£_type  =  UUV_DIS__pdu->entity_type; 

t:irrie_stamp_of_current_PDU  =  lJUV_DIS_pdu*'>enci ty_stat:e_header .  time^stainp; 
tirrie_of_PDU_receipt  =  clock  (); 

PDU_overdu€  =  FALSE; 


//  Posture 

AOT_x 

AU\’_y 

AIT\^_2 

ALW_phi 

AU*'v^_thGta 

ALV_psi 


UUV_DIS_pdu->entity_location.x  ♦  FT_PER_METERS; 
UUV_DIS_pdU'>entity_location.y  *  FT_PER_METERS; 
UW_DIS_pdu->entity_Iocation.  z  *  FT_PER_METERS; 
radians  {UUV_DIS_pdu->enti ty_orientation .phi ) ; 
radians  {UUV_DIS_pdu->enti ty_orientation . theta ) ; 
radians  {UUV_DIS_pdU“>enti ty_orientation .psi ) ; 


cout  «  -viewer:  DIS_net_read  posture  trace:"  «  endl 


cout  « 
cout  « 
cout  « 
cout  « 
cout  << 
cout  « 
cout  << 


UUV_DIS_pdu->entity_Iocation .X 
UUV_DIS_pdu->entity_Iocation .y 
ULrv_DIS_pdu->ent  i  ty_iocation .  z 
UU\' _D  I  S_pd  u  -  >  e  n  t  i  t  y  _o  r  i  e  n  t  a  t  i  o  n .  ph  i 
UUV_DIS_pdu->entity_oriGntation. theta 
UIJ\7_DI  S_pdu  -  >en  t  i  ty_or  i  ent  a  t  i  on .  ps  i 


]  "  «  endl  j 


/'  Linear  and  angular  velocities  in  body  coordinates/meters  by  DIS  standard 
AlA_x_dot  =  Uirv_DI£jdu->entity_velocity .X  *  FT_PERJ^ETERS; 

AU’v_y'_dot  =  UUV_DIS_pdu->entity_velocity .y  *  FT_PER_METERS ; 

A/Jv^r^dot  =  UUV_DIS_pdu~>entity_velocity .  z  »  FT_PER_METERS  ; 

AW_p-hi_dot  =radians  fUUV_DIS_pdU“>dead_reckon ^paraits  .  angular_velocity  [  0]  )  ; 

AL^_:iie:a_doc  =  radlan3  {ULn^__DIS_pdu->dead_reckon__paraTns  .  angular_velocity  [  1  ]  )  ; 

AUV_psl_dot  =radians (UUV_DIS_pdu->dead_reckon_params . angular_velocity [ 2 ] ) ; 


UUV_DIS_pdu->ent ity_velocity .X  «  “ , 

UlJV_DIS_pdu->entity_velocity  .y  « 

UUV_DI£_pdu->entity_veloci  ty  .  z  «  ; 

UU\^_DIS_pdu->dead_reckon_parains .  angular_veloci  ty  [0]  «  ", 
UU\^_DI£_pdu>>dead_reckon_parains .  angular_velocity  [1]  «  ",  •; 
LnJV_DI£_pdu->dead_reckon_parains . angular_velocity  [2]  «  "]  " 


// 

cout 

<-/ 

/  / 

c  ou  t 

« 

/  ; 

cout 

«' 

cou  t 

« 

/  / 

cout 

<< 

cout 

;  / 

cout 

« 

/  / 

cou  t 

« 

/  / 

If 

cout 

« 

//  Note  that  even  though  the  accelerations  are  calculated  in  the  superclass 
//  UUVBody,  use  of  global  state  vector  lets  us  construct  a  class  hierarchy 
//  based  on  problem  structure  instead  of  the  communications  dependencies. 

//  This  is  proposed  as  a  general  DIS-compatible  vehicle  object  hierarchy. 

//  Accelerations  are  not  produced  in  world  coordinates,  thus  zeroes  expected 
AUV_u_dot  =  UUV_DIS_pdu->dead_reckon_params . linear_accel  [0]  *  FT_PER_HETERS; 
AUV_v_dot  =  UUV_DIS_pdu->dead_reckon_params  .linear_accel  [1]  *  FT_PERJ4ETERS; 
AUV_w_dot  =  UUV__DI S_pdu~>dead_reckon_pa rams . linear_ac cel  (2]  *  FT_PER J4ETEES ; 

//  cout  «  "viewer:  World  coordinate  accelerations: 

//  cout  «  •  1  •  ; 

//  cout  «  UUV_DIS_pdu*>dead_reckon_params . linear„accel  [0]  «  ■,  •; 

//  cout  «  UUV_DIS_pdU“>dead_reckon_params .  linear_accel  [1]  «  •  ] 

//  cout  «  UUV_DIS_pdu->dead_reckon_params.linear_accel  [2]  «  “]■  <<  endl; 

//  what  we  look  like 
//  UUV_DIS_pdu->entity_appearance; 

//  UUV_DI£_pdu->entity_inarking.  characterise t; 
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/  /  UUV_DI  S_pdu->entity„iTiarking  .markings  ; 

//  project  our  movement 

UUV_DI S_pdu->dead_reckon_pa rams . algorithm  =  DRAlgo_DRM_FPW; 

//  cout  <<  "viewer:  DIS_net_read  ()  successful"  «  endl ; 

//  cout  «  flush; 

//  update  overall  AUV  posture  (both  position  &  orientation) 
AU\^_position_node->translat ion. setValue  {  AUV_x,  -  AUV_y,  -  AUV_z); 
rot ate_AUV_z->  angle. setValue  (  -  AUV^psi); 

rctate_AUV_^->  angle .  setValue  (  -  AUV_theta)  ; 

rctate_AUV_x->  angle. setValue  (  AUV_phi ) ; 

ArticulatParamsNode  ♦  APNptr  =  UUV_DIS_pdu->articulat_params_head; 

AW_time  =  APNptr->articulat_params  .parameter_value  (0)  ; 

AUV_time  +=  APNptr->articulat_params .parameter^value  [1]  /  10.0; 

AW_delta_rudder  =  APNptr->articulat_params  .parameter_value  [2]  ; 

AW_del  ta_planes  =  APNptr->articulat_params  .parameter^value  13]; 

//  denormal i 2e  former  shorts  to  be  negative  if  necessary 
if  (AW_del ta_rudder  >=  128.0)  AUV_deita_ rudder  -=  2S6.0; 
if  (AUV_deIta_planes  >=  128.0)  AUV’_del ta_planes  -=  2B6.0; 

AJV_port_rpm  =  APNpt r->articulat_params .parameter_value  [4]; 

if  i AUV_port_rpm  >=  128.0)  AUV_port_rpm  =  AUV_port_rpm  -  256.0; 

A  VV_p  o  r  t  _rpiTi  *  =  10.0; 

i  f  ( Airv_pcrt_rpm  >=  0.0) 

AW_port_rpm  *t=  APNptr“>articulat_params ,parameter_value  [5]; 
else  A'«J\/_port_rpm  -=  APNptr->articulat_params . parameter_value  [5]; 

AW_£tbd_rpir;  =  APNptr->articulat_j)arams  .parameter_value  [6]  ; 

if  ( AUV_stbd_rpm  >=  128.0}  AUV_stbd_rpm  =  AUV_stbd_rpm  -  256.0; 

ALV_s  t  bd_rpm  ♦ =  10.0; 

i f  ( ALW^s t  bd_rpm  >=  0.0} 

AUV_stbd_rpir.  +=  APNptr->articulat_params  .parameter_value  [7]; 
else  AU\'_stbd_rpm  -=  APNptr“>articulat_params .parameter_value  [7]; 

//  cout  <^<  "viewer:  Articulation  parameter  0:"  «  endl; 

cout  "AW_tlme  =  "  «  AU\^_time  «  endl; 

//  cout  «  "AtV_del ta^rudder  =  "  «  AUV_delta_r udder  «  endl; 

//  cout  "AUV_delta_planes  =  "  «  AUV_delta_planes  «  endl ; 

//  cout  <<  "AWjort_rprr.  =  "  «  AUV_port_rpm  «  endl ; 

//  cout  «  "AUV_stbd_rpm  =  "  «  AUV_stbd_rpm  «  endl  ; 

APNptr  =  APNptr->next_articulat_params;  //  next  articulated  parameter 

//  thrusters 

AlT\‘_bow_vertical  =  APNptr->articulat_params  .parameter_value  (0]; 
AUV_sterri_vertical  =  APNptr->articulat_params  .parameter_value  (!]; 
AUV_bow_lateral  =  APNptr->articulat_params .parameter_value  [2] ; 
AUV^stern^lateral  =  APNptr->articulat_params .parameter_value  (3]; 


//  denormalize  former  shorts  to  be  negative  if  necessary 


if 

{ AUV_bow_ver  t  i  c  a  1 

>= 

128.0) 

AUV 

.bow_vertical 

-=  256.0 

if 

{AUV_stern„vertical 

>= 

128.0) 

AUV. 

.stern_vertical 

-=  256.0 

if 

( AUV_b ow_l a  t  e  r a 1 

>= 

128.0) 

AUV. 

_bow_lateral 

-=  256.0 

if 

( AUV_s  te  rn_l a  t  era 1 

>= 

128,0) 

AUV. 

.stern^lateral 

-=  256.0 

//  convert  thruster  volts  to  force  by  signed  squares  &  normalize  adjust 
AUV_bow_vertical  =  AUV_bow_vertical  *  f abs {AUV_bow_vertical  )  /  24.0 

AUV_stern_vertical  =  AUV_stern_vertical  *  f abs (AUV_stern_vertical )  /  24.0 

AUV_bow_lateral  =  AUV_bow_lateral  *  f abs {AUV_bow_lateral  )  /  24.0 

AUV_stern_lateral  =  AUV_stern_lateral  *  f abs (AUV_stern_lateral  )  /  24.0 

//  slots  4.. 7  as  yet  unused 
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//  ccuc  «  "Viewer:  Articulation  parameter  1:  thrusters" 
/  /  <<  endl ; 


/  /  cout  <■<  "AUV_bow_vertical 
//  cout  <<  "AUV_stern_vertical 
//  cout  "AW_bow_lateral 
'  c  0  Lu  t  •  •  A  1 6 1' a  t  €;  r  a  1 


«  AUV_bow_vertical  «  endl; 
«  AUV_stern_vertical  «  endl; 
«  AUV_bow_lateral  «  endl ; 
<<  AW_stGrn_lateral  «  endl; 


APNpcr  =  APNptr->next_articulat_params;  //  next  articulated  parameter 


AUV_ST1 0  0  0_bea  r i ng 

AUV_£T1000_range 

AUV_ST1000_strength 

AW_ST725_bearing 

AUV_ST7 2 Strange 
AW_ST7  25_strength 


APNpt  r  -  >art  iculat_paraTns .  pa  ramete  revalue 
APNpt  >art i culat^params . parameter^value 
APNpt  r->art icul at^params . parameter^value 
APNpt r->articulat_params . paramGter_value 
APNpt  r->articulat_parains .  pa  ramete  revalue 
APNptr>>articulat_params,parameter_value 
APNpt r~  >art i cul at^params . pa ramete revalue 
APNpt  r-  >articulat_parajns .  pa  ramete  revalue 


[0]  *  10  + 
[1]; 

[2]  /  4.0; 

[3] ; 

[4]  *  10  + 

[5] ; 

[6]  /  4.0; 

[7]  ; 


// 

// 

// 

// 

// 

// 

// 


cout  << 
cout  « 
cout  << 
cout  << 
cout  << 
cout  << 
cout  << 


“viewer:  Articulation 
''AUV_ST1000_bearing  = 
“AUV^STl 000_range  = 
*'AUV_ST1 000_strength  = 
“AUV_ST725.bearing 
"AUV_ST7  2  Strange 
“AUV_ST725_strenath  = 


parameter  2:  sonar"  «  endl ; 

«  AUV_ST1000_bearing  «  endl ; 
"  «  AUV_ST1000_range  «  endl  ; 
<<  AUV_ST1000_strength  «  endl; 
«  AUV_ST72  5__bearing  «  endl; 
<<  AUV_ST725_range  «  endl; 
<<  AUy_ST725_strength  «  endl; 


/'  / 


Fi^nc  hostname  of  PDU  {revision  in  network . round  version) 

=  getnostbyaddr  f  ( char  *>  itinaddr,  sizeof  ( struct  in  addr) 
cout  <'<  -viewer:  Host  name:  -  «  hp“>h_name  «  endl  ; 


AF_INET)  ; 


//  don't  forget  freePDU  or  get  a  memory  leak! 
• /  articulated  parameters  are  also  freed 


freePDU  (local^PDU) ; 


//  cout  - 
//  end  whi 


viewer:  freePDu  ilocal^PDU)  called  for  this  PDU"  «  endl 
infinite  loop 


“Viewer:  DIS  nec.read  porcion  complete,  now  update  scene  graph." 


turient^ci Gok 
deita_cIock 
de I ta_time 


C] OCk  ( ) ; 

current__cl ock  -  time_^of_^PDU _ receipt; 

(double)  delta_clock  /  CLOCKS_PER_SEC; 


// 
!  ! 
if 

if 

{ 


;ut  -viewer:  current_clock  =  " 

<<  ",  time_stamp_of_current_PDU  = 
'"<  ",  time_of__PDU_ receipt  =  - 
«  ",  PDU  delta_time  =  " 


«  current_clock 
«  time_stamp_of_current_PDU 
<<  t  ime_of_PDU_receipt 
«  delta_time  «  endl  ; 


(  (delta_time  >=  0.0)  &&  (delta^time  <=  5.0)) 


delta_x 

delta_y 

delta_2 


=  AUV_x_dot 
=  AUV_y_dot 
=  AUV_z_dot 


delta_time; 

delta^time; 

delta_time; 


del ta_phi 
del ta_theta 
delta_psi 


=  AUV_phi_dot 
=  AUV_theta_d 
=  AUV_psi_dot 


*  del  ragtime; 

*  delta_time; 

*  delta_time; 


//  update  positions,  postures 


// 

cout 

« 

// 

« 

// 

cout 

« 

// 

« 

// 

cout 

« 

// 

« 

"viewer:  DeadReckon_Callback :  " 

“  PDU  delta_time  =“•  «  delta^time 


AUV_phi 

AUV_phi_dot 

AUV_theta 

AUV_theta_dot 


«  degrees  (AUV_phi) 

«  degrees  (AUV_phi_dot ) 

«  degrees  (AUV_theta) 

«  degrees  (AUV_theta_dot ) 


«  endl; 
«  endl; 
«  endl; 
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«  delta_x 
«  delta_y 
«  delta_z 


«  endl  ; 
«  endl ; 
«  endl; 


//  cout  «  •*  delta_x 
//  cour  «  "  delta_y 
//  cout  «  "  delta^z 
//  couc  «  end! ; 

//  save  current  position  for  next  time  the  camera  is  repositioned 
AW_x_prior  =  AUV_x; 

AlJV_y'_prior  =  AUV_y; 

AUV_z_prior  =  AUV_z; 

//  graphics  rendering  problems:  psi,  rudders  are  opposite 

//  update  overall  AUV  posture  (both  position  &  orientation) 
AUV_position_node->translation.setValue  (  AUV_x  +  delta_x, 

-  (AUV_y  +  delta_y), 

-  (AUV_2  +  delta_z)); 

rotace_AW_2->angle .  setValue  (  >  {AUVjsi  +  delta_psi)); 

rotate_AUV_y->angle. setValue  (  -  (AUV_theta  +  delta_theta) } ; 

rotat6_AUV_x->angie .  setValue  (AUV_phi  -»■  delta_phi); 

if  update  AW  rudder  &  plane  orientations 

rotar  e_AW_bow_rudders-vangle .  SGtValue  (  radians  (AUV_delta_rudder)  )  ; 
rota te_AUV_stern_rudderS“>angle. setValue  (  -  radians (AUV_del ta_rudder) ) ; 
rctate_AW_bow_planeS“>angle  .  setValue  {  -  radians  (AUV_delta_planes)  )  ; 
rc!:ate_AW_stern_planes->angle  .  setValue  (  radians  (AUV_delta_p lanes)  ) ; 

//  Tout  <<  "AlTV_dGlta_ rudder  =  "  «  AUV_delta_rudder 
if  «  ",  radians  (AUV^del ta_rudder )  =  • 

.//  <<  radians  { AUV_del ta_rudder ) 

«  endl; 

//  ccut  <<  "AW_delra_planes=  "  «  AUV_delta_p lanes 
//  <>'  ,  radians  {AW_del ta_p lanes }  =  " 

//  «r  radians  f AUV_del ta_planes ) 

/  /  <<  endl  ; 


if  ( f abs (AUV_stbd_rpm  - 

(coneFropGllerWa)ceStbd->height .getValue ( )  *  700,0/24.0))  >  10.0) 
//  ensure  needed 
{ 

conePropeilerWa)ceStbd->height  =  AUV_stbd_rpm  /700. 0*24.0; 

conGPrope21erWakeStbd->bottomRadius  =  fabs  (AUV_stbd_rpm)  /700.0*  6.0; 

if  { f abs (AUV_port_rpm  - 

(con€PropellerWa)cePort->h€ight  .getVaiue  ( )  *  700.0  /  24.0))  >  10.0) 
//  ensure  needed 

t 

conePropellerWa)cePort->height  =  AUV_port_rpm  /700. 0*24.0  ; 

conePropellerWakePort->bottomRadius  =  fabs  (AUV_port_rpm)  /700.0*  6.0; 

) 

if  ( fabs (AUV_bow_verti cal- ( thrusterWakeFV->height .getValue 0 /2 . 0 ) )  >  1.0) 
//  ensure  needed 

j 

thrusterWakeFV->height  =  -  AUV_bow_vertical  *  2.0; 

thrusterWakeFV->bottomRadius  =  AUV_bow_vertical  /  4.0; 
if  {AUV_bow_vertical  <  0.0) 

//  bottomsideBow,  negative  volts  push  up  (negative  direction) 
whichWakeFV->whichChild  =  1; 

//  topsideBow,  positive  volts  push  down  (positive  direction) 
else  whichWakeFV->whichChild  =  0; 

topsideBow->  translation. setValue (0 ,  TOPHALF+AUV_bow_vertical , 0) ; 
bottomsideBow->translation. setVaiue(0,  BOTTC»4HAliF+AUV_bow_vertical , 0) ; 

) 

if  (fabs (AUV_stern_vertical- (thrusterWakeAV->height .getValue ( ) /2 .0) ) 

>  1.0) 

//  ensure  needed 
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{ 

thrusterWakeAV->height  =  -  AUV_stern_vercical  *  2.0; 

tririJsterWakeAV->bottom.Radius  =  AUV_stern_vertical  /  4.0; 
if  (AUV_stern_vertical  <  0.0) 

//  boccomsideStern,  negative  volts  push  up  (negative  direction) 
whichWakeAV->whichChild  =  1; 

//  topsideStern,  positive  volts  push  down (positive  direction) 
else  whichWakeAV->whichChild  =0; 

copsideStern- >  t lanslation . setValue (0 ,  TOPHALF+AUV_stern_vertical , 0 ) 
^DOttomsideStern->translation.setValue (0,BOTTOMHALF+AUV_stern_vertical, 0) 

if  (fabs  ■ALtv’_bcw_laceral- (thrusterWakeFH->height  .getValue( )  /2.0))  >1.0) 

!!  ensure  needed  =  =>  > 

{ 

tl-.rjsterWakeFH->height  =  -  AW_bow_lateral  *  2.0; 

thrusterWakeFH->bottomRadius  =  AUV_bow_laceral  /  4.0; 
if  (AIT ;?_bow_l ateral  <  0.0) 

//  rightsideEow,  negative  volts  push  left  (negative  direction) 
whi chWaKeFH~>whichChi Id  =  1; 

^  //  leftsideBow,  positive  volts  push  right  (positive  direction) 

e^se  wnichWakeFH->whichChild  =  0;  ^ 

lef tsideBow->  translation . setValue  (  0,  LEFT_HALF+AUV_bow_lateral ,  0) ; 
rightsideBow->translation.setValue  (  0,  RIGHTHALF+AUV_bow_lateral ,  0); 

if  i  f  abs  I  AU\'_sterrj_l  ateral  -  ( thrusterWakeAH->height .  get  Value  ( )  /2 . 0 )  ) 

'>  1.0) 

./  ensure  needed 

thrusterWakeAK->height  =  -  AU\’_stern_lateral  *  2.0; 

t.'.rjsterWakeAH->boctoinF,adius  =  AUV_stern_lateral  /  4.0; 
if  (AlT’v’_ster,n_l ateral  <  0.0) 

.  ^^S-btsideStern ,  negative  volts  push  left  (negative  direction) 
w.ni  chWakeAK->whichChi  Id  =  1; 

//  lef tsideStern,  positive  volts  push  right (positive  direction) 
else  whichWakeAH->whichChild  =  0; 

i  e.  ^ a i dei  ^ern-  translati on .  setValue  { 0 ,  LEFT_HALF+AUV_stern_l ateral ,  0 ) ; 
rigntsiCieCtern- -.translation .setValue  (  0 , RIGHT.HALF.AU'/_stern_lateral ,  0)  • 

tans  .A,'.  _£T.o0i._range  -  cone£cnarST1000->height  .getValue  ()  )  >  0.0) 

/■  ensure  needed 


.t.cneSonar£TI000->height  =  fabs  (AUV_ST1000_range)  ; 

cone£onar£T1000->bottoinRadius  =  fabs  (AUV_ET1000_range)  /  60.0; 

//  1  degree 

xfCcne£onarS71000->translation.setValue  (  0.0, 

-  (AUV_ST1000_range  /  2.0), 
4.0  /  12.0  ); 


else  if  (PDU_overdue  ==  FALSE)  //  update  scene  graph,  reset  vehicle  position 

PDU_overdue  =  TRUE;  //  over  5  seconds  elapsed  since  last  PDU 

//  restore  latest  valid  AUV  posture  {both  position  &  orientation) 
AUV_position_node->translation . setValue  (  AUV_x, 

-  AUV^' 

-  AUV_z )  ; 

rota te_AUV_2 ~>angle . setValue  (  -  AUV_j}si); 

rotate_AUV_y>>angle . setValue  (  -  AUV_theta); 

rotate_AUV_x->angle. setValue  {  AUV_phi ) ; 

//  thrusters 

AUV_bow_ve  rtical  =  0.0; 

AU\^_stern_vertical  =  0.0; 


138 


0.0; 

0.0; 


AUV’^bo  w_  lateral 
AUV_stern_lateraI 


rhrusterWakerv-'height  =  o.O 
chruscerWakeFV~>bottoniRadius  =  0.0 
thrustei'WakeAV->height  =  0.0 
thru5CerWakeAV->bottoinRadius  =  0.0 
thrusCerWakeFH“>height  =  0.0 
thrusterWakeFH“>bottoiTiRadius  =  0.0 
thrusterWakeAH->height  =  0.0 
thrusterWakeAH->bottoitiRadius  =  0.0 


AUV_ST1000_bearing  =  0; 

AUV^STl 00 Derange  =  0.0; 

AUV_ST1000_strength  =  0; 

AU\^_ST725_bearing  =  0; 

AU\'_£T7  2  Strange  =  0.0; 

AW_ST7  2  5_st  length  =  0; 

coneSonar£T1000->height  =  f abs (AUV_ST1000_range) ; 

coneSonarST1000->bottomRadius  =  f abs (AUV_ST1 000_range) /60 . 0 ;  //  1  degree. 

AUV_port_rpTr.  =  0.0; 

AUV_stbd_rpir.  =  0.0; 

conePropel lerWakePort->height  =  0.0; 

conePropel  lerWakePcrt~>bot  toinRadius  =  0.0; 
conePropel IerWakeStbd->height  =  0.0; 

conePropelIerWakeStbd->bottoiTiRadius  =  0.0; 

ccut  <<■  “viewer;  DeadReckon^Callback :  “ 

"PDU  delta_time  =  “  <<  delta_tiine  «  * ,  " 

<<  endl ; 

couc  <<  “viewer  posi tion/pcsture  reset  to  last  received  PDU."  «  endl; 
ccut  <<  end! ; 

COUl  <<  flush; 


if  giobals 
priorAUVPosi tion 
currentAUVPosi ti on 
aheadOfAUVPosi tion 


SbVec3f  (AUV_x_prior,  AUV_/_prior,  AUV_z_prior) ; 
SbVec3f  (AUV_x,  AUV_i^,  AUV_2)  ; 

currentAUVPosition  + 

SbVec3f  (10.0*cos  (AUV_psi),  10 . O^sin {AUV_psi ) , 2 . 0) ; 


switch  (  whichCamera  )  //  reposition  appropriate  cainera  as  needed 
case  CAMERA_FREE: 


priorCainera Posi tion  =  PerspecciveCameraFree->position.getValue  (); 
priorCameraOf f set  =  priorCameraPosit ion  -  priorAUVPosi tion; 

currentCameraPosi tion=  priorCameraPosition; 

PerspectiveCaineraFree->orientation.getValue (orientationRotationAxis, 


break ; 


orientationRotationAngle) 


case  CAMERA_TO_AUV :  //  retain  cainera  pos'n  relative  to  new  AUV  position 
priorCameraPosition  =  PerspectiveCameraToAUV->position.getValue  (); 
priorCameraOf fset  =  priorCameraPosition  -  priorAUVPosition* 

//  verify  here 

currentCameraPosition  =  (  currentAUVPosition 

.  .  +  standardCameraOffset  ); 

PerspectiveCajneraToAUV->posataon.setValue  (  currentAUVPosition 

+  StandardCameraOffset  ) ; 

PerspecciveCameraToAUV->pointAt  (currentAUVPosition  )• 

PerspectiveCameraToAUV->focalDistanc€.s€tValue(standardCameraFocalDistance) • 
PerspectiveCameraToAUV-’>orientation.getValu€  (orientationRotationAxis, 

orientationRotationAngle  ); 
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case  CAM£RA_FROM__AUV :  //  retain  camera  position  looking  out  from  AUV  pos . 
priorCameraPosition  =  PerspGctiveCameraFromAUV->position.getValue  {) 
priorCameraOf f set  =  priorCameraPosition  -  priorAUVPosi tion; 

//  verify  here 

currentCameraFosi tion  =  (  currentAUVPosi tion  ) ; 

PerspectiveCameraFromAUV->position . setValue  {  currentAUVPosi tion  ) ; 

PerspectiveCameraFromAUV->pointAt  {  currentAUVPosi tion 

+standardCameraOf f set  ) ; 

PerspectiveCameraFroiriAUV->  focal  Distance .  setValue  (standardCameraFocalDi  stance) 
Perspect  1  veCameraFromAU\^-->oriencat  ion .  getValue  (orientationRotationAxis , 

orientationRotationAngle  ); 

break ; 

)  //  end  switch  (  whichCamera  ) 


:int  out 
«  endl 


<<  endl , 
-dell 

H  H 

«  “  ,  - 

«  endl , 
<<  -del: 


all  camera  parameters 


AUV_position 


a_AUV_position 


=  <-  «  (AUV_x) 
«  (AUV_y) 
«  (AUV_z) 


«  (AUV_x  -  AUV_x_prior) 

«  {AU\^_y  -  AUV__y_prior) 

«  (AUV_z  -  AUV_z_prior)  « 


^CameraPosition  =- 

fcurrentCameraPosition  [0] 


:  (currentCameraPosition  [1]  -  pric 
:  (currentCameraPosition  [2]  -  prio 
:  endl ; 

“priorCameraPosition  =" 

'  priorCameraPosition  [0] 

:  priorCameraPosition  [1] 

■  priorCameraPosition  [2]  «  ->-  << 
-priorCameraOf f set  =- 

'  priorCameraOf f set  (0] 

•  priorCameraOf f set  [1] 
priorCameraOf f set  [2]  «  •>"  «  e 
“currentCameraPosition  =“ 

■  currentCameraPosition  [0] 
currentCameraPosition  [1] 

■  currentCameraPosition  [2]  «  ->- 
"crientationRotat ion  = - 

orientationRotat ionAxis  [0] 
orientationRotationAxis  [1] 
orientationRotationAxis  [2] 
degrees  (orientationRotationAngle 


priorCameraPosition  [0]) 
priorCameraPosition  [1] ) 
priorCameraPosition  [2] ) 


<<  endl; 


«  endl  ; 


endl  ; 


«  endl ; 


“viewer:  end  of  DIS_Redraw_Callback 


«  endl; 


return; 


//  called  on  an  exit  condition  via  a  call  to  atexit  (DlS^net^close)  in  main 

static  void  DIS_net_close  () 

{ 

cout  «  -viewer:  DIS_net_close  ();-  «  endl; 
net^close  (); 

DIS_port_op€n  =  FALSE; 


. 


//////////////////////////////////////////////////////////////////////////////// 
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//  This  is  called  by  the  Color  Editor  whenever  the  color 
//  has  changed.  The  userData  is  set  by  main()  in  the  call 
/■/  to  ScXtColorEdicor:  :addColorChangedCailback. 

VO;.  d 

coIorEditorCB  I  void  *userData,  const:  SbColor  *rgbCallbackData  ) 

SoXtRenderArea  ^renderArea  =  (SoXtRenderArea  *)  userData; 
renderArea->setBackgroundColor (  *rgbCaIlbackData  ) ; 


//////////////////////////////////////////////////////////////////////////////// 

SoSeparator  *  readFile (const  char  *filename)  //  Inventor  Mentor  p.  284 

//  Open  the  input  file 

Solnput  inyScenelnput  ; 

i f  : InyScenelnput . openFile (filename) ) 

couc  •  <  “Cannot  op^en  file  “  <<  filename  <<  endl  ; 
return  NUbL; 

//  Read  the  whole  file  into  the  database 
^cSeparator  *  myGraph  =  SoDB readAll (iuny Scene Input ) ; 

:f  .myGraph  ==  NULL) 

cout  «  "Problem  reading  file  "  «  filename  «  endl ; 
return  NULL; 

} 

my Scene Input .closeFile  ( ) ; 
return  m.yGraph; 


!  !  !  !  i  !  f  !  ' 


!  I  n !  1 1 1 1  a  1 1 1 1  i  1 1  n  1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 


void  initialize^Qlobals  \) 

1 

//  multicast  port  &  group 
strncpy  (port,  "3111",  4); 

strncpy  (group,  "224.2.121.93",  12); 


//  3111  is  npsnet  'standard'  port 
//  # define  DEFAULT_PORT  =  "3111"; 

//  #define  DEFAULT_GROUP  =  "224.2.121.93"; 

//  initialize  materials 

silver->  ambientColor . setValue  (  .2,  .2,  .2  ); 

siIver->  diffuseCoIor.setValue  (  .6,  .6,  .6  ); 

silver->specularColor. setValue  (  .5,  .5,  .5  ); 

sHver->shininess  =  .5; 

gold-^  ambientColor . setValue  (  .4,  .2,  .0  ); 

gold->  diffuseCoIor.setValue  (  .9,  .5,  .0  ); 

gold->specuIarColor . setValue  {  .7,  .7,  .0  ); 

goId“>shininess  =  .6; 


brass->  ambientColor . setValue  (  0.329412,  0.223529,  0.027451  ); 
brass“>  diffuseCoIor.setValue  {  0.780392,  0.568627,  0.113725  )• 
brass->specularCol or .setValue  (  0.992157,  0.941176,  0,807843  )] 
brass- >shininess  =  0.21794872; 

chrome->  ambientColor . setValue  (  0.25,  0.25,  0.25  ); 
chrome->  dif  fuseColor .  setValue  {  0.4,  0.4,  0.4  ),* 

chrome->specularColor .setValue  (  0.774597,  0.774597,  0.774597  ); 
chrome->shininess  =0.6; 
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npsblue->  ambientColor . setValue  (  0.0,  0.0,  1.0  ); 

npsL'i-je->  di  f  f  useCci  or  .  setValue  (  0.0,  0.0,  0.8  }; 

npsbIue->specularCoior .setValue  (  0.0,  0.2,  1.0  )• 

npsblue->shrniness  =0.8; 

seagreen-->  ambieDtColor .  setValue  {  0.0,  0.5,  0.0  }; 

seagreen->  di  f  fuseCoior .  setValue  {  0.0,  0.5,  0.0  )  ;' 

se5creen->specul6rColcr . setValue  (  0.0,  0.5,  0.0  ); 

seagreen- >shininess  =0.0; 

darkgreen-'>  aitbiep.tCcl  or .  setValue  (  0.15,  0.20,  0.15  ); 
oarf.green--*  di  ffuseColor .  setValue  (  0.15,  0.20,'  0,15  j; 
darKgreen->sp€cularColor . setValue  (  0.15,  0.20,  0.15  )• 
darkgreen- >sriininess  =  0.0;  ^  •  // 

3o:ia:_red->  ambi  entCol  or .  setValue  (  1.0,  0.0,  0.15  ); 
soriai-_ied~>  di  ffuseColor .  setValue  (  i.O,  0.0,  0.15  ); 
3Gnar_red->specularColor  .setValue  (  1.0,  0.0,  0.15  ) 
sonai^red- >shininess  =  0,0; 


Initialize  aiobals  M 


U ;  !  i  / !  !  I !  i  !!  i  !  i  i  !  1 1 !  I !,!  n  i  N!  I !!  I !  f  I !  1  i  !  f  /  f 
'  /  /  /  •'/■'///////////////////  /  7  ///  ,^  ///////////  / 

_coniT:iana_Iine_f lags  fint  argc,  char  **  argv) 
If  comtiarid  line  arguments 


///////////////, 


:  i.ndex,  i; 

COUt  “  [ 


<  “  [parse_corTimand_line_flags  start:  #  arguments  =  "  <<  arac  «  "  r- 

<  end.: ; 


7  -  0;  1  <  argc;  i+-»-)  ccut  <<  argv  [i]  <<  " 

;  "  «  endl ; 

=  1;  i  <  argc;  i^+) 

5  j-iidex  =  0;  index  <=  strlen  (argvfi]);  indGx+  +  )  //  uppercase 

argvfij  [index]  =  toupper  (argv[i]  [index]}; 

7strcmp  iargv[ij,  "PORT")  ==0}  tl 

(strcmp  (argvii],  “-port")  ==  0^  li 

(strcmp  (argvii],  "F'm  ==  0)  I! 

'strcmp  'argvfi],  "-P")  ==  O) ) 

if  (  i-^i  >=  argc  ) 

COUt  <'<  "Insufficient  parameters  for  PORT"  «  endl  * 
e^  se  ' 

{ 

COUt  «  •'["  «  argv[i]  «  -  "  «  argv[i  +  l]  «  -  ]  *  «  endl  ; 

strcpy  {port,  argv[i+l]}  ; 
i-*--f  ; 


(  (strcmp 

(argv[i] , 

•GROUP" } 

==  0) 

(strcmp 

{argv[i] , 

•-GROUP" ) 

==  0) 

(strcmp 

(argv[i] , 

“G") 

==  0) 

(strcmp 

(argvii] , 

•-G") 

=  =  0) 

(strcmp 

{argv[i] , 

•ADDRESS" ) 

==  0) 

(strcmp 

( a  rgv [ i ] , 

•-ADDRESS"  ) 

=  =  0} 

(strcmp 

(argv[i] , 

-A") 

==  0) 

(strcmp 

(argvii) , 

•-A") 

==  0}) 

if  (  i+1  >=  argc  ) 

COUt  «  "Insufficient  parameters  for  GROUP  ADDRESS."  «  endl  ; 
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else  cout  <<  •'Unrecognized  command  line  parameter:  «  argv[i] 

<<'  ignored.*'  «  endl ; 

}  //  end  for  loop  through  command  line  parameters 

//  cour  <-<'  '•  [parse_command_line_f  lags  complete]"  <<  endl  ; 

return; 

I  //  end  parse_command_line_f lags  {) 


'U'/// A'  /  //////////////////////////////////////////////////////////////////////// 

:  ^  ^  '  i ! I i i i i i ! U ! i 1 i i i i i n ! I n i ! i n ! i ! i !/ i U i II i {//  j  j  j ! j / i ! f  n i j / j / I/I f ! I i I !/ f I j 

main  i  int  argc,  char  **  arqv  ) 

//  Initialize  Inventor  and  Xt  -  these  steps  MUST  be  first  calls 
//  in  main,  without  exception,  or  a  mystery  crash  results. 

Widget  ViewerWindowWidget  =  SoXt : : init (argv[0] ) ; 
if  I  ViewerWindowWidget  ==  NULL  } 

cout  «  “viewer:  ViewerWindow^Widget  ==  NULL  on  startup,  exiting." 

«  endl ; 
exi t (  1  }  ; 

} 

cout  '■:<  "viewer:  ViewerWindowWidget  added"  «  endl; 

i n i t i a  1 i ze_g 1 oba 1 s  (); 

par3e_command_line_f lags  (argc,  argv) ; 

//  port  and  group  can  change  by  command  line  switches 
cout  '■  "multicast  port  =  "  <<  port 

",  address  group  =  "  <<  group  «  endl; 

cour  -  "creating  the  scene  graph:"  <<  endl  ; 

root  ^  new  SoSeparator; 
root->ref ( ; ; 

cout  <<'  "root  added"  <<  endl; 

/ /  correct  for  different  coordinate  system  -  not  yet  working 
//  SoRotatio.nXYZ  *  coordina teSy stemFl ip  =  new  SoRotationXYZ; 

//  coordin3te£ystemFlip->angle . setValue  (  M_PI  ); 

//  coordinateSystemFlip->  axis . setValue  {  SoRotationXYZ: :X  ); 

//  root->addChild {  coordinateSys temFlip  ); 

SoRota ti onXYZ  *roOx  =  new  SoRotationXYZ; 

roOx-eangle . setValue  (  0.0  }; 

ro0x->  axis . setValue  (SoRotationXYZ : :X) ; 

SoRotationXYZ  *ro90x  =  new  SoRotationXYZ; 
ro90x->angle . setValue  (M_PI  /  2.0); 
ro90x->  axis . setValue  (SoRotationXYZ : :X) ; 

SoRotationXYZ  *ro270x  =  new  SoRotationXYZ; 
ro270x->angle .  setValue  (3.0  *  M__PI  /  2.0); 
ro270x~>  axis. setValue  (SoRotationXYZ : :X) ; 

PerspectiveCameraFree  =  new  SoPerspectiveCamera; 

PerspectiveCameraToAUV  =  new  SoPerspectiveCamera; 

PerspectiveCameraFromAUV  =  new  SoPerspectiveCamera; 

//  can't  put  a  group  or  separator  above  camera 
//  cameras  using  pointAt  are  90  degrees  twisted  askew 
//  Create  a  camera  SoSwitch  node 
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SoSwitch  *  whichCameraSwitch  =  new  SoSwitch; 
rocc->addChild  (  whichCameraSwitch  ) ; 

whichCameraSwi tch->addChild  (  Perspect iveCameraFree  ) ; 
whichCameraSwi tch->addChild  (  Perspect iveCameraToAUV  ) ; 
whichCameraSwi tch->addChild  (  Perspect iveCameraFromAUV  ); 

//  default  whichCamera  defined  at  top 
whichCameraSwi tch->whichChi Id  =  whichCamera; 

//  'Create  a  camera  rotation  correction  SoSwitch  node 
SoSwitch  *  whichCameraCorrectionSwitch  =  new  SoSwitch; 
root->addChi Id  {  whi chCameraCorrectionSwitch  ) ; 
whichCameraCorrectionSwitch->addChild  (  roOx  ); 
whichCameraCorrectionSwi tch->addChild  (  ro270x  ); 
whichCameraCorrectionSwi tch->addChiId  (  ro270x  ); 
whichCameraCorrectionSwi tch->whichChild  =  whichCamera; 

root->addChild{  new  SoPointLight  }; 

ScTransfcrm  *  xfLight2  =  new  SoTransform; 
xf  Light2 -‘>translat  ion .  setValue  (0 . 0 ,  0.0,  -  30.0); 
root-'>addChi  Id  (  xfLight2  ); 
root->addC.hiid  (  new  SoPointLight  ); 

SoTransform  *  xfLight3  =  new  SoTransform; 
xfLight5->translation. setValue {0 .0,  0.0,  3C.0); 

roc: ->addChiid i  xfLightS  ); 
cout  "2  point  lights  added  "  «  endl ; 

SoXt RenderArea  *  myRenderArea  =  new  SoXtRenderArea  (ViewerWindowWidget ) ; 
StV:ev;portReg:on  myRegion  (myRenderArea- >getSi  ze  ()  }  ; 

currentACTCPositicn  =  SbVec3f  (AUV_x,  AUV^/ ,  AUV_2)  ;  //  globals 

aneadOf  AsJ'/Posi  t  i  on  =  currentAUVPosi  tion  + 

SbVec3f  (10.0  *  cos  (AUV_psi),  10.0  *  sin  (AUV_psi),  0.0); 
//  global 

//  Free  (urimodi  f  ied)  Camera 

PerspectiveCameraFree->viewAli  (root,  myRegion,  1.0);  //  global 

Perspect iveCameraFree->aspectRatio . setValue  (SO_ASPECT_VIDEO) ; 

//  Camera  that  keeps  AUV  in  center 

PerspectrveCameraTo.AW->viewAll  (root,  myRegion,  1.0);  //  global 

Perspect iveCameraToAUV^->aspectRatio. setValue  (SO_ASPECT_VIDEO) ; 
PerspectiveCameraToAUV->posi tion . setValue 

(  currentAUVPosition  +  standardCameraOf fset  ) ; 
//  PerspectiveCameraToAUV->or lent at ion. setValue 

(SbRotation  (1.0,  0.0,  0.0,  M_PI  /  2.0  )); 
PerspectiveCameraToAUV->pointAt  (  currentAUVPosition  ); 

//  Camera  that  looks  out  from  AUV  in  center 

PersrjectiveCameraFromAUV->viewAll  (root,  myRegion,  1.0);  //  global 

Pe r spec tiveCameraFromAUV->aspectRatio. setValue  (SO_ASPECT_VIDEO) ; 
FerspectiveCameraFromAUV->position. setValue  (  currentAUVPosition  ); 

//  perspectiveCameraFromAUV->orientation. setValue 

{SbRotation  (1.0,  0.0,  0.0,  M_PI  /  2.0  )); 
PerspectiveCameraFromAUV->pointAt  (  currentAUVPosition  +  standardCameraOf fset  ); 

unitsfeet  =  new  SoUnits; 

uni tsfeet->uni ts . setValue  {  SoUnits :: FEET  ); 
root->addChild (  unitsfeet  ); 

SoPickStyle  *  unpickablestylenode; 

SoPickStyle  *  pickablestylenode; 
unpickablestylenode  =  new  SoPickStyle; 

pickablestylenode  =  new  SoPickStyle; 
unpi ckablesty lenode->style . setValue  (  SoPickStyle: lUNPICKABLE  ); 
pickablestylenode->style. setValue  (  SoPickStyle: :BOUNDING_BOX  ) ; 
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SoCube  *  backgroundCube  =  new  SoCube; 
backgroundCube->width  =  400.0; 
ba:KgroundCube-'>heighC  =  4  00.0; 
backgroundCube->depth  =  0.1; 


ScTransform  *  xf backgroundCube  =  new  SoTransform; 
xf  backgroundCube->translat:ion  .  setValue  (0 . 0 ,  0.0,  “50.0); 

SoTexcureC  *  overhangTexture  =  new  SoTexture2; 
overhangTexture“>f ilename . setValue  { "overhang. rgb" ) ; 

SoSeparator  *  sepbackgroundCube  =  new  SoSeparator; 
sepbackgroundCube->addChiId (  pickablesty lenode  ) ; 
c.epbackgroundCube-->addChild(  unitsfeet  )  ; 
seoGackqroundCube-'>addChild  (  darkgreen  ); 
if  (TEXTURE  ==  TRUE) 

sepbackgroundCube->addChild (  overhangTexture  ); 
sepbackgroundCube->addChi Id (  xfbackgroundCube  ) ; 

seprackaroundCube- >addChild (  backgroundCube  );  //  remove  cube  here 

roc:  - --addCni  id  (  sepbackgroundCube  ); 


/  /  Jason  with  engine  animation 
EcEeparator  *  sepJason  =  new  SoSeparator; 

ScTransform  *  xfJason  =  new  SoTransform; 

SoSeparator  *  Jason  =  readFile  ("Jason.iv"); 

xfJason->translation. setValue (  -60.0,  40.0,  -  25.0);  //  center  of  pattern 

sepJason->addChi Id  (  xfJason  ); 

//  animation  from  Inventor  Mentor  13 . 6 .Calculator , C++ 

//  Set  up  the  Jason  transformations 
SoRotaticnXYZ  *  danceRotate  =  new  SoRotationXYZ ; 
danceRotate-vangle . setValue  {  0.0  ); 
danceRotate->  axis . setValue  {  SoRota t ionXYZ : : Z  ); 
sepJasori->addChild  (  danceRotate  ); 

SoTranslation  ^  danceTranslate  =  new  SoTranslation ; 
sepJascn->3ddChi Id  (  danceTranslate  ); 
sep.:asan-->addChi  Id  (  Jason  ); 
root--  addChild  (  sepJason  ); 

//  Set  up  an  engine  to  calculate  the  motion  path: 

:  :  Theta  is  incremented  using  a  time  counter  engine, 

//  and  converted  to  radians  using  an  expression  in 
i!  the  calculator  engine. 

SoCalcul ator  *  caic>:Y  =  new  SoCal cula tor ; 

ScTTmeCounter  *  thetaCounter  =  new  SoTlmieCcunter; 
thetaCounter->max  =  360; 
thetaCounter->step  =  1 ; 

thetaCounter->f requency  =  1  /  180.0;  //  180  seconds  for  a  full  cycle 
calcXY->a  .  ccnnectFrom  { thetaCounter- >output )  ; 

calcXY->expression . set  1 Value (0 ,  “ta=a*M_PI/180 " ) ;  //  theta  (radians) 

calcXY->expression . setlValue (1 ,  " tb=l 5 *cos ( ta) " ) ;  //  r,  z 

calcXY->expressi on . set  lvalue {2 ,  " td=tb*cos ( ta) " ) ;  //  x 

calcXY->expression . setlValue (3 ,  “ te= tb*sin ( ta) " ) ;  //  y 

calcXY->expression . setlValue (4 ,  "oA=vec3f ( td, te, tb) " ) ;  //  vector  output  A 
danceTranslate->translation . connectFrom {&calcXY->oA) ; 

calcXY->expression. setlValue (5,  "ob=2*ta");  //  scalar  output  b 

danceRotate->angle . connectFrom (&calcXY->ob) ; 

//  Platform 

SoSeparator  *  sepPlatform  =  new  SoSeparator; 

SoTransform  *  xfPlatform  =  new  SoTransform; 

SoSeparator  *  Platform  =  readFile  ("Platform.iV); 
xf Plat form-> translation . setValue (-80.0,  50.0,  -50.0); 
sepPlatf orm->addChild  (  xfPlatform  ) ; 
sepPlatf orm->addChild (  Platform  ); 
root->addChi Id  {  sepPlatform  ) ; 
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bcSepci ra uor  *  sepTestcank  =  new  SoSeparator; 

SoTransf orin  *  xfTesttank  =  new  SoTransf oriri; 
SoSeparacor  *  Testtank  =  readFile  f'Testtank.iv'*); 
xfTesctank->translacion.secVaIue (  0.0,  0.0,  -50.0); 
sepTest:t:ank->addChiid  (  xfTesttank  )  ; 
sepTe?rtank-'->addChild(  Testtank  ' 
root ->addChi Id  (  sepTesttank  ) ; 


//  Torpedo  Tube 

SoSeparator  *  sepTorpedoTube  =  new  SoSeparator; 
SoTransf orm  *  xfTorpedoTube  =  new  SoTransf orm; 


xfTcrpedoTube->translation . setValue {  30.0, 
SCO/ Under  *  TorpedoTube  =  new  SoCylinder; 


20.0,  -48.00  ) ; 


//  21“  diameter 
//  10'  length 


TorpedcTube->radius  =  21.0  /  12.0; 

TorpedcTube->height  =  10.0; 

TorpedoTube->parts  =  SoCylinder :: SIDES;  //  let's  drive  through' 
sepTorpedoTube->addChild  {  xfTorpedoTube  ); 
sepTorpedoTube->addChild (  TorpedoTube  ); 
root-v>addChild  (  brass  ); 
root ->addChi Id  (  sepTorpedoTube  ); 


SoSeparator  *  AW_node  =  makeAUVc; 


root -'>addChi Id  {  AUV_node  );  //  AUV  object  creation  routine  above 


//  WriteAction  writes  scene  graph  to  file 
FILE  *  auv_iv_fp  =  fopen  {“auv.iv",  “w'*); 
S oWr 1 1  eA c  t i on  wr 1 1 ea  c  t i on ; 


w  r  1 
w  r  t 


^acc  ton  .  qetOurpu  t  ■')->  setFi  leFointer  (auv_iv_fp)  ; 
ea c 1 1  on . apply  { AUV_n ode } ; 
se  (auv_iv_fp) ; 

•'  "  wrt  teac  1 1  on  .  apply  (AUL^_node)  =>  auv.iv  complete." 


«  endl ; 


//^WriteAction  writes  scene  graph  to  file 

FILE  ^  3uv_vw_iv_fp  =  fopen  ( *' auv_vw^  iv“  ,  *'w’‘); 

wri teact ion . getOutpu t { ) ->  setFilePointer  (auv_vw_iv_fp); 

wri t eact ion . apply  (root); 

f  c  I  o  s e  •'  a  u  v_^n^’_l  f  p  :■  ; 

ccut  "wri  teact  I  on  .  apply  (root)  =>  auv_vw.iv  complete."  «  endl  ; 


DIS_net_open  ( ) ; 

atexit  (DIS_net_close) ;  //  ensure  port  is  reclosed  on  exit,  tested  sat. 

current_clock  =  clock  ();  //  initialize 


//  A  Timersensor  updates  the  object  with  DIS  postures  and  performs  redraws 
ScTransform  *  dummy _xf orm  =  new  SoTransf orm; 

SoTimerSensor  *  DIS_Redraw_Sensor  =  new  SoTimerSensor (  DIS_Redraw_Callback 


f'l£_Fedraw_Sensor->setInterval  (  0.10  };  //  seconds 

DIS_Redraw_Sensor->schedule  (); 


dummy _xform  )  ; 


//  system  {“rm  sounds /nps__auv .  au ")  ; 

//  system  ( "www  -o  sounds /nps_a uv. au 

file: / /taurus . cs.nps. navy .mil /pub/auv/nps_auv,au“ ) ; 

//  system  (“www  -o  sounds/nps_auv . au 

htcp://www_tios.cs.uCwente.nl/say/?Naval+Postgraduate+School,Autonomous+Underwater+Veh 

r  c  1  e  )  ; 

//  system  (“sfplay  sounds /nps_auv . au  &“); 
if  (PRINTDIALOG  ==  TRUE) 

//  Print  dialog  widget:  Inventor  training  manual  p,  9-9 
SoXtPrintDialog  *printDialog  =  new  SoXtPrintDialog; 
printDialog->setSceneGraph  (root) ; 
printDialog->show  {); 

1 
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/*  colcreditor  not  found?! 

if  (BACKGROUNDCOLORDIALOG  ==  TRUE) 

{ 

//  Buiid  the  color  editor  in  its  own  window 

SoXtColorEdi tor  *color_edi tor  =  new  SoXtColorEditor; 
ccIor_editor~>bui Id { ) ; 

coIor_edi tor->setTrtlG (  -AUV  viewer  background  color"  ); 

.  Add  a  callback  for  when  the  color  changes 

color_edi t or->addCol orChangedCal Iback (  colorEditorCB/  //  the  callback 

viewer  );  //  user  data  to  be  passed 

SbColor  lightbluecclor {  .0,  .5,  .75  ); 

viewer->setBackgroundColor (  lightbluecolor  ); 
col or^edi tor->setColor  (  lightbluecolor  ) ; 
ccIor_edi tor-'^'Show  ( )  ;  //  Display  the  color  editor 


//  UncorruTient  which  viewer  you  want  to  use: 

SoXr.Exatui-ierViewer  *  viewer  =  new  SoXtExaminerViewer ; 

//  SoXt r lyViewer  *  viewer  =  new  SoXtFlyViewer; 

E'-X:  n  3i;eViewe:  *  viewer  =  new  SoXtPlaneViewer  ; 

//  SoXtWa 1 kviewer  *  viewer  =  new  SoXtWalkYiewer ; 


v: -  .'SecScerjeGraph  (  root  ); 
viewer-->3etTitle  '  "NPS  AUV  Virtual  World"); 

V I  e  w  e  r  -  >  s  h  ow  ( )  ; 

//  Xt Real izeWidget  ■:  ViewerWindowWidget  );  //  mini  window  junk 

ScXt : imainLcop ( ) ;  //  loop  forever,  sending  events  to  the  scene  graph 

} 

//  end  cf  view^er.C 

I !  U  i !  i ! i ! i i ! I ! i n  I ! I N ! I II I f i ! I f ! i ! I ! i  U  t !  1 1 f ! n ! i ! i f i n i i t ! i II I ! i ! i i i ! I i ! i i U i 
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i::i  Q 


c. 


Makefile  for  Object-Oriented  Real-Time  Graphics  Viewer 


'#!siTiake 

PROGRAM  =  viewer 
C-^-^FILES  =  viewer. C 

#  /’asr /include/make/commondef s  and  /usr/include/make/coinmonrules 

#  define  some  useful  Makefile  rules.  For  example,  they 

#  defines  a  'clean'  target,  so  you  can  type  'make  clean' 

#  to  remove  all  .o  files  (or  other  files  generated  during 

^  compilation) .  See  the  file  /usr /include/make/commonrules  for 
^  documentation  on  what  targets  are  supported. 

include  /usr/include/make/commondef s 

TARGETS  =  $ (PROGRAM) 

OBJECTS  =  viewer. o 

^  Libraries  to  link  with: 

LLDLIBS  =  -lInventorXt 

4,^  ########################################################## 

#  LIS  includes 

IN-rS  =  -  I  .  . /DIS  .mcast /h 

-  -L  .  . /DIS  .mcast/src 

LIp,  =  .  . /DIS  .mcast/src/iibdis„client .  a 

LIE_ARG  =  -ldis_client 

R _ PATH  =  ♦ . / Di S . me as t / h / 

RS  =  $ (HDR_PATH)pdu.h  $ (HDR_PATH) disdef s . h 

default:  $ (TARGETS) 
include  $ (COMMONRULES) 

$ ( TARGETS )  :  $ ( OBJ  ECTS ) 

CC  $(C++FILES)  -o  $@  $ (OBJECTS)  $(LDFLAGS)  $ (LIB)  $(LIB_DIR)  $(INCS)  \ 
$ (LIB_ARG}  $ (LLDLIBS) 


#  modified  from  pat  ton : /usr/share/src/Inventor/samples/clock/Makefile 

#  updated  2  October  94  Don  Brutzman 
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V.  UNDERWATER  VIRTUAL  WORLD  HYDRODYNAMICS 
A.  Introduction 

Structuring  the  model  design  problem  was  the  key  to  comprehensible 
implementation.  A  straightforward  hierarchy  follows.  Posture  is  common  to  all 
vehicles  and  can  be  represented  either  by  Euler  angle  rotations,  by  a  homogenous 
transformation  matrix  (Fu  87)  (Foley,  van  Dam  90),  or  by  quaternions  (Cooke  92). 
Forces  and  accelerations  acting  upon  a  rigid  body,  if  modeled,  are  related  by  dynamics 
equations  of  motion  corresponding  to  each  spatial  degree  of  freedom.  A  rigid  body  is 
further  subject  to  kinematics  equations  of  motion  which  combine  velocities  with 
postures  in  strictly  defined  ways  regardless  of  vehicle  type  or  environmental 
dimensionality.  A  networked  rigid  body  which  communicates  with  other  entities  via 
DIS  needs  to  calculate  postures,  optional  linear  and  rotational  velocities,  and  (again 
optional)  linear  accelerations  (IEEE  93).  Such  a  DlS-networked  rigid  body  has 
identical  capabilities  regardless  of  vehicle  type. 

An  entity  dynamics  component  for  a  real-time  networked  virtual  world 
combines  the  functionality  of  rigid  bodies  and  DIS  networking  with  the  dynamics 
equations  of  motion  (forces  and  accelerations)  unique  to  a  specific  vehicle  type.  This 
structured  hierarchy  of  relationships  between  posture  representations,  rigid  bodies, 

DIS  networking  and  dynamics  equations  of  motion  led  to  the  general  model  class 
diagram  which  appears  in  Figure  1. 

The  compartment  boxes  within  Figure  1  delineate  the  functionality  of  class 
components.  The  first  compartment  is  class  name.  The  second  compartment  indicates 
member  data  fields,  which  are  the  data  structures  encapsulated  by  the  object.  The 
third  compartment  indicates  object  methods  (functions)  which  effectively  occur 
instantaneously.  The  fourth  compartment  includes  methods  (functions)  which  are 
time-consuming,  either  from  the  perspective  of  simulation  clock  duration  or  actual 
delay  due  to  network  latency.  Adapted  from  the  Object-Oriented  Simulation  Pictures 
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(OOSPICs)  design  and  testing  methodology  (Bailey  94),  this  diagraming  approach  is 
very  useful  because  it  simplifies  presentation  of  key  object  relationships  and  clarifies 
hierarchy  design.  Of  particularly  value  is  the  explicit  specification  of  temporal 
relationships,  which  are  critical  to  success  in  a  real-time  system  and  are  often 
overlooked  in  complex  system  design.  An  example  object  template  which  adapts  the 
OOSPICs  methodology  from  MODSIM  programming  language  to  C++  appears  as 
Figure  2.  A  key  for  OOSPIC  arrow  conventions  is  included  in  Figure  3  (Bailey  94). 
Although  C++  OOSPICs  are  not  provided  for  each  class,  inspection  of  specifications 
in  the  accompanying  source  code  reveals  that  every  class  follows  the  structural  layout 
presented  in  Figure  1. 
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Hydrodynamics  Model  Class  Hierarchy 


UUVBody _ 

body  accelerations 
mass  matrix 

initialize 

invert  mass  matrix 
integrate  equations  of  motion 


virtual  world  interface 
dynamics  response  I 

d^  reckoning  response 


inherits 

IS-NetworkedRigidBodV 


DIS  port  connection  data 
Protocol  Data  Unit  (PDU) 
time_of_last_PDU 

initialize  and  inspection 
connect  and  disconnect 
send  PDU 


peananent 

ownership 


temporary 

ownoship 


UUVModel 

hydrodynamics 
ZDodel  coefficients 


AUVSocket 

communications  with 
networked  AUV 


AUVglobals 

telemetry  state  vector 
alternately  iq)dated  by 
robot  and  m^l 


stay>alive  send  PDU _ 

inherits 

RigidBody 

quaternion  oriwitation 
h-transform-matrix  posture  - 
posture:  position,  orientation 
body  velocities 


Not  shown:  VectorSD  class 


HTransfonnMatrix 
horizontal  transform 
matrix  with  posture 


ternion 


q$  qt  qi  qs 
(euler  parameters) 


rotate 

(step  or  incremental) 
operators 

inflection  methods 


Figure  1.  General  real-time  DIS-networked  hydrodynamics  model  class  hierarchy. 


Class  Name: 

Inherits: 

viadtility  pmpose  ramne. 

Qsput  pammetos)  (ratum  type}  dendptioD 

member  data  fields 

private 

public 

instantaneous  methods 

pdvite 

tmie*€Oii5uixiing  methods 

pfivate 


public 


ffrpatyin  or  twriiiiiftirty  OVQDl, 

idmuhtiOD  clock  or  win  dock 


Figure  2 


OOSPIC  class  diagram  template  for  C++ 
class  definitions.  Separation  of  class 
name,  data  fields,  instantaneous 
methods  and  temporal  methods  clarifies 
class  functionality  and  design. 


inheritance  pmnanent  tenqwmiy  membcnh^ 
ownership  ownership 


Figure  3 . 


Object-Oriented  Simulation 
(OOSPICs)  arrow  conventions. 


Pictures 


The  structure  of  the  general  real-time  DlS-networked  dynamics  model 
presented  here  appears  to  be  applicable  to  vehicles  of  arbitrary  type.  Documented 
source  code  for  each  member  of  the  class  hierarchy  matches  the  equations  and 
algorithms  presented  in  this  work,  and  also  follows  the  structure  appearing  in  Figure  2 
(Brutzman  94).  Future  work  of  interest  in  model  design  includes  directly  porting  this 
model  to  emulate  the  characteristics  of  other  underwater  vehicles,  adapting  the  model 
to  accommodate  dissimilar  vehicle  entities,  porting  the  model  into  robot  software  as  an 
on-board  hydrodynamics  response  predictor,  and  investigating  extensions  to  the  model 
to  support  visualization,  validation  and  verification  of  model  relationships  against 
archived  or  live  data  records  of  actual  vehicle  dynamics  performance. 


154 


B.  dynamics.C  Virtual  World  Interface  and  Top-Level  Hydrodynamics 

The  dynamics  program  is  the  server  connection  to  the  underwater  virtual 
world.  It  must  be  started  prior  to  the  robot  execution  program  so  that  the  robot  has  a 
virtual  world  to  connect  to.  dynamics  also  provides  a  user  interface  for  setting  ocean 
currents,  multicast  network  parameters  and  hydrodynamics  class  library  test  routines. 
The  user  menu  appears  in  Figure  4. 

The  dynamics  program  has  a  text-only  user  interface  in  order  to  reduce 
processor  loading,  maximize  portability  and  facilitate  remote  operation.  The  menu 
option  to  reset  multicast  group  parameters  permits  starting  a  virtual  world  channel 
using  collision-free  parameters  automatically  chosen  by  the  MBone  sd  session 
directory  application.  The  menu  option  for  ocean  currents  accepts  a  constant  3D 
vector.  Extending  the  overall  functionality  of  the  underwater  virtual  world  is 
accomplished  by  building  hooks  between  new  models  (such  as  time-varying  ocean 
currents)  and  the  dynamics  program.  New  models  can  either  be  embedded  directly 
within  dynamics,  or  can  be  distributed  and  communicate  via  sockets. 
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dude: /n/dude/work/brutzman/dynamics»  dynamics 


Dynarriics  classes  test  selections 

L  loop_test_with_execution_level  (); 

M  Multicast  parameter  input 

ttl=15,  group  address=224.2 . 121. 93 ,  port=3111 
O  Ocean  current  vector  reset 
<0,  0,  0> 

H  Hmatrix/guaternion  exerciser 

R  Rotation  of  cjuaternion  &  Hmatrix  using  pgr 
D  Defaults 
I  Invert  matrix  test 

E  dEad_reckon_test_with_execut ion_level 
P  PDU_skip_interval  change  (from  1) 

T  Toggle  TRACE  =  0 
C  DIS_net_close  ( ) ; 

Q  Quit 

Enter  choice:  _ 


Figure  4.  dynamics  virtual  world  server;  user  interface. 


//////////////////////////////////////////////////////////////////////////////// 

/* 

Program:  dynamics. C 

Description:  hydrodynamics  model  and  virtual  world  manager 

Author:  Don  Brutzman 

Revised:  18  October  94 

System:  Irix  5.2 

Compiler:  ANSI  C++ 

Compilation:  irix>  CC  dynamics, C  -Im  ~w  -g  -o  dynamics 

[or]  unix>  make  dynamics 

+w  ==  Warn  about  all  questionable  constructs. 

Advisors:  Dr.  Mike  Zyda,  Dr.  Bob  McGhee  and  Dr.  Tony  Healey 

Lessons:  Need  single  char  input  without  requiring  <CR> 

Don't  use  'static'  for  member  data  fields  or  mysterious  linker 
eRrors  result 
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Don't  use  <<  endl  «  endl  more  than  once  in  a  single  cout  call 
or  a  vague  warning  results  with  an  invalid  line  number: 
'warning:  ostream&os cream: ;operator  « (ostream& ( *) 
(ostream^))  not  inlined,  called  twice  in  an  expression' 


//////////////////////////////////////////////////////////////////////////////// 

# include  <stdio-h> 

^include  <ctype.h> 

^inciude  "UUVBody.C" 

^include  <bstring.h> 

/////// ///////////////////////////////////////////////////////////////////////// 
//  gioiial  data 

char  port  [6];  //  pointer  to  MULTI CAST_PORT 

cftar  group  [20];  //  MULTICAST_GROUP; 

-nt  int_ttl  =  15;  //  time-to-live 

char  choice  =  //  dummy  value  to  print  menu  first  time 

int  READ_MENU_CHOICE  =  TRUE; 


^  !  U  i! !  ; ;  !  I !  I  i  !  i  !  !  I  !  I !  !  I  n  i  (  (  !  / 1 !  !  ( I H I  ( t  !  !  !  {  !  !  !  f  !  I  (  i  !  !  u  !  (  m  !  I !  !  I  (  !  f  i  i  !  i  I !  I !  {  ! 

void  parse_command_line_f lags  (int  argc,  char  **  argv) 

//'Command  line  arguments 

I 

int  index,  i; 

//  3112  is  npsnet  'standard'  port 
strncpy  (port,  "3111“,  4); 

strncpy  'group,  “224.2.121.93",  12); 

••/  cout  <-<  “  lparse_cominand_line_flags  start:  #  arguments  =  "  «  argc 
//  endl; 

//  cout  «  " [  “  ; 

//  for  (i  0;  i  <  argc;  if+)  cout  «  argv  [i]  «  “  - ; 

//  cout  «  "]"  <<  endl  ; 

for  (i  =  1;  i  <  argc;  i++) 

{ 

for  (index  =  0;  index  <=  strlen  (argv[i]);  index++)  //  uppercase 
argv[i]  [index]  =  toupper  (argv[i]  [index]); 


II  cout  «  " [parse_command_line_flags  uppercase:  #  arguments  =  "  «  argc 
//  «  "  ]  “  «  endl  ; 

//  cout  «  "  [  "  ; 

//  for  (i  =  0;  i  <  argc;  i++)  cout  «  argv  [i]  «  " 

//  cout  «  "]“  «  endl; 


for  (i 
{ 

if 


{ 


1;  i  <  argc;  i++) 


(  (strcmp 
(strcmp 
(strcmp 
(strcmp 


(argv[i]  , 
(argvli]  , 
(argvii]  , 
{argv[i]  , 


"TTL*)  ==  0)  II 
--TTL-)  ==  0)  II 
“T")  ==  0)  II 
--T")  ==  0)) 


if 


(  i+1  >=  argc  ) 

cout  «  "Insufficient  parameters  for  TTL"  «  endl; 
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el  se 


int_ttl  =  atoi  {argv[i+l]}; 

couc  <<  "dynamics:  tcl  reset  to  "  <<  int_ttl 
«  endl; 


[  (strcmp  (argv[i],  "PORT")  = 
(strcmp  (argv[i],  “-PORT")  = 
{strcmp  {argvii],  "P")  = 

(strcmp  {argv[i],  "-P")  = 


=  0)11 
=  0)11 
=  0)11 
=  0)  } 


if  (  i^l  >=  argc  } 

cout  «  "Insufficient  parameters  for  PORT"  «  endl; 

el  se 
{ 

bzerc  (port,  6) ; 
strcpy  (port,  argv[i+l]); 

cout  «  "dynamics:  port  reset  to  "  «  port 
<*"  endl; 


strcmp 

( a  rgv  f 1 1 , 

"GROUP" ) 

=  =  0) 

strcmp 

( a  rgv [ i ] , 

"-GROUP" } 

=  =  0) 

strcmp 

( a  rgv [ i ] , 

"G" ) 

=  =  0) 

strcmp 

( a  rgv [ i ] , 

"-G"  ) 

=  =  0) 

strcmp 

( a  rgv [ i ]  , 

"ADDRESS" ) 

0) 

strcmp 

(a rgv [ i ] , 

"-ADDRESS" } 

=  -  0) 

strcmp 

{ a  rgv ( i ] , 

“A"  ] 

=  =  0) 

s  t  r  cmp 

(a rgv [ 1 ] , 

"-A") 

==  0) 

(  i+i  argc  ) 

cout  <<  "Insufficient  parameters  for  GROUP  ADDRESS"  «  endl; 


bzero  (group,  20) ; 
strcpy  (group,  argv[i+l])  ; 

cout  «  "dynamics:  group  address  reset  to  "  «  group 
«  endl; 


else  if  ((strcmp  (argv[i],  "LOOP")  ==  0)  I1 

(strcmp  (argv[i],  "-LOOP")  ==  0)  il 

(strcm.p  (argviij,  “L")  ==  0)  !l 

(strcmp  (argvfi],  "-L")  ==  0)) 

choice  =  ' L' ; 

READ_MENU_CHOICE  =  FALSE; 

cout  <<  “dynamics:  Loop  test  with  execution  level  set  “  «  endl; 

} 

else  if  ((strcmp  (argv[i],  "TRACE")  ==  0)  M 
(strcmp  (argv[i],  "-TRACE")  ==  0)) 

{ 

TRACE  =  TRUE; 

cout  «  "dynamics:  TRACE  is  now  on"  «  endl; 


cout  «  "dynamics:  unrecognized  command  flag  "  «  argv[i]  «  endl; 

} 

//  end  for  loop  through  command  line  parameters 
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//  cour  «■  "  [parse_cominand_line_f lags  complete]"  «  endl; 
return; 


j  /  /  end  parse_coiTLT!and_line_f lags  {) 

!!  I  n  i !  I !  I !  t  n  I !  I !  I !  I !  I !  I !  I !  I !  I !  I !  I !  t !  I !  I !  I !  I !  I !  I H  !  I !  I !  u  !  I !  I !  I !  I !  I H I !  I !  I H I !  I 

void  main  (  int  argc,  char  argv  ) 

double  al, 

xl , 

p,  a,  r,  delta_time; 


argv  ) 

bl , 

cl, 

yi. 

21, 

q/ 

r, 

int 
i  n.  c 

Vector 3D 
Quaternion 
Hmatrix 
RigidBcdy 

D I SN  e  t  w  o  r  k  edR  i  g  i  dB  ody 
UlT/Body 


done  =  FALSE ; 
index,  repetitions; 

vector!,  vector2; 

quaternionl,  quaternion2; 

hmatrixl,  hmatrix2; 

rigidbodyl ; 

disnetworkedrigidbodyl ; 
uuvbodyl ; 


//  main  start 


i:.a:  se_comr.and_::ne_f  lags  (argc,  argv)  ; 


:f  (  i  isspace  (choice}) 
{ 


c  cu  t 

end] ; 

ccut 

endl ; 

cout 

"Dynamii 

cs  classes 

test  selections" 

« 

endl ; 

cout 

L 

loop_test_with_execution_level  ( } ; " 

« 

endl ; 

cout 

<< 

" 

M 

Multicast  parameter  input" 

« 

endl ; 

cout 

« 

" 

ttl="  «  int_ttl  «  ",  group  address=" 

« 

group 

<< 

\  port="  «  port 

« 

end! ; 

cout 

<'<• 

" 

0 

Ocean  current  vector  reset  " 

« 

endl ; 

c  ou  t 

<"  <<  AUV_oceancurrent_u  «  ",  " 

«  AUV_oceancurrent_v  «  ",  " 

«  AUV_oceancurrent_w  «  ">  " 

« 

endl; 

cout 

« 

M 

H 

Hmatrix/quaternion  exerciser" 

« 

endl  ; 

ccut 

<< 

M 

R 

Rotation  of  quaternion  &  Hmatrix  using 

P  c 

3  r  " 

« 

endl  ; 

cout 

« 

II 

D 

Defaults" 

« 

endl  ; 

cout 

« 

H 

I 

Invert  matrix  test" 

« 

endl  ; 

cout 

« 

li 

E 

dEad_reckon_test_wi th_execution_level • 

« 

endl  ; 

cout 

« 

« 

P 

PDU_skip_interval  change  (from  " 

« 

uuvbodyl. PDU_skip_interval_value  ()  «  •)• 

« 

endl  ; 

cout 

« 

tl 

T  Toggle  TRACE  =  "  «  TRACE 

« 

endl; 

cout 

« 

« 

C 

DIS_net_close  ( ) ; " 

« 

endl; 

cout 

<< 

M 

Q 

Quit" 

« 

endl ; 

cout 

« 

"Enter 

choice: 

cout 

« 

flush ; 

if  (READ_MENU_CHOICE  ==  TRUE) 
{ 
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//  still  not  reading  a  single  char  properly  : 
//  cin.get  (choice); 
cin  >>  choice; 
cout  <<  endl ; 

} 

else  cout  «  choice  «  endl; 

READ_MENU_CHOICE  =  TRUE; 
switch  (choice) 


case  'L' :  case  ' 1 ' :  //- 


cout  <<  *’Loop  test  with  execution  level”  «  endl; 


uuvbodyl . UUVBody_initiali ze 
uuvbodyl . sec_ttl 
uuvbodyl . set_group 
uuvbodyl  .  set_jport 


0  ; 

(int__ttl  j 
(group) ; 
(port) ; 


cout  «  “d>'namics;  uuvbody . loop_test_wi th_execution_level  ()"  «  endl 

uuvbodyl . loop_test_wi th_execution_level  ( ) ; 

cncjce  =  //  dumiTiy  value  to  print  keyboard  menu 


case  'o':  //- 


cout  <<  "  Ocean  current  vector  reset  "  «  endl ; 

cout  " _ "  <<  endl  ; 

cout  <<  end!  ; 

cout  <<  "Enter  AUV_oceancurrent  in  North  direction:  ” ; 

cin  >>  ALTv^oceancurrent^u ; 

cout  <<  "  new  Atrv^_oceancurrent_u  =  "  «  AUV_oceancurrent_u  «  endl  ; 
cout  <<  endl; 

cout  <<  "Enter  AUV_oceancurrent  in  East  direction:  " ; 

c:ri  >•.-  AU\^_oceancurrent_v; 

cout  «  "  new  AU\'_oceancurrent_v  =  "  «  AUV_oceancurrent_v  «  endl  ; 
cout  «  endl; 

cout  <•<  "Enter  AUV_oceancurrent  in  Downward  direction:  "  ; 
cin  »  AUV_oceancurrent_w; 

cout  "  new  AUV_oceancurrent_w  =  "  «  AUV_oceancurrent_w  «  endl  ; 
cout  endl; 


choice  = 
break ; 


//  dummy  value  to  print  keyboard  menu 


case  "M' :  case  'm' :  //- 


cout  «  "  Multicast  parameter  input  "  «  endl; 


cout  <<  “ _ 

cout  endl; 


/  «  endl; 


cout  «  "Enter  time  to  live  ttl:  •* ; 
cin  »  inc_ctl; 

cout  «  "  new  time  to  live  ttl  =  "  «  int_ttl  «  endl; 
cout  <<  endl ; 


cout  «  "Enter  group  address: 
cin  »  group; 

cout  «  "  new  group  address  =  "  «  group  «  endl; 
cout  «  endl; 
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couc  «  "Enter  port:  " ; 

cin  >>  port; 

cout  <<  "  new  port  =  “  <<  port  <<  endl ; 

cout  endl; 

clioice  =  //  dummy  value  to  print  keyboard  menu 

break ; 

case  'H':  case  'h' :  // - 

cout  «'  "Hmat rix/guaternion  exerciser:"  <<  endl ; 

cout  «  "Enter  euler  angles  in  degrees  for  object: 
cin  »  al  »  bl  >>  cl; 

cout  «*  "  angles  entered  =  <"  <<  al  <<  " /  "  <<  bl  <<  " ,  "  «  cl  « 

<<  endl; 

quaternionl  =  Quaternion  (radians  (al),  radians  (bl),  radians  (cl)); 

cout  "quaternionl:  "; 

quaternionl .print  ( ) ; 
quaternionl .normalize  () ; 

cout  «  endl; 

cout  «  "normalized:  "  <<  quaternionl  «  endl ; 

vector2  =  qua ternionl . euler_angles  (); 

cout  «  "euler  anglesl:  "  «  vector2  «  endl ; 

cout  «  "phi  theta  psil:"  «  quaternionl .phi_value  ()  «  ",  • 

<<  quaternionl . theta_value  ()  <<  ",  “ 

<<•  quaternionl . psi_value  ()  «  endl  ; 

cout  "degrees:  "  <<  degrees  (quaternionl  .phi_value  ())  «  ",  " 

<<  degrees  (quaternionl . theta_value  ())«",  " 
<<  degrees  (quaternionl .psi_value  ())  «  endl 

cout  *'■<'  endl; 

cout  «  "Enter  vector  positions  for  object:  " ; 


cin 

X 

y 

V 

>  2 1  ; 

vector! 

=  VectorBD 

(xl,  yl,  Zl)  ; 

cout  << 

" vector 1 : 

"  <<  vectorl  «  endl; 

hma t  rixl 

=  Hmatrix 

(  vectorl,  radians  (al),  radians  (bl),  radians 

(cl) )  ; 

cout 

"hmat  rixl : 

"  <<  hmatrixl  «  endl ; 

cout  <'< 

"  hmatrixl 

angles:  "  <<  degrees  (hmatrixl .phi_value  ()) 

<x  degrees  (hmatrixl . thGta_value  ()) 

«  degrees  (hmatrixl .psi_value  ()) 

«  " ,  " 
«  " ,  " 
«  end! 

choice  = 
break ; 

// 

dummy  value  to  print  keyboard  menu 

case  'R' :  case^r':  // - 

cout  «  "Rotation"  «  endl; 
cout  «  endl; 

cout  «  "Enter  euler  angles  in  degrees  for  object: 
cin  »  al  »  bl  »  cl; 

quaternionl  =  Quaternion  {radians  (al),  radians  (bl),  radians  (cl)); 

cout  «  "Enter  rotation  rates  in  degrees  per  sec  <p,  q,  r>: 
cin  »  p  »  q  »  r  ; 
p  =  radians  (p) ; 
q  =  radians  (q) ; 
r  =  radians  (r) ; 
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couc  «  "Enter  repetitions  and  deita^time  in  seconds: 
cin  »  repetitions  >>  deita_time; 

cout  <<  "Enter  vector  positions  for  object: 

cin  >>  xl  »  yl  »  zl; 

vector!  =  VectorBD  (xl,  yl,  zl); 

hmatrixl  =  Hmatrix  (vectorl,  radians  (al) ,  radians  (bl),  radians  (cl)); 
hiT^atrix2  =  Hinatrix  (vectorl,  radians  (al),  radians  (bl),  radians  (cl)); 

for  (index^O;  index  <=  repetitions;  index++) 

{ 

cout  <<  "quaternion  =  "; 
quaternionl .print  (); 
cout  «  endl ; 

cout  «  "quaternionl  angles  =  <" 

«  degrees  (quaternionl . phi_value  ())  «  ",  " 

«  degrees  (quaternionl . theta_value  ())  «  ",  " 

«  degrees  (quaternionl . psi_value  ())  «  ">  "  «  endl; 

qua ternionl  .  increiTiental^rotate  (p,  q,  r,  delta_time)  ; 

cout  «"  "  hmatrixl  angles  =:  <"  «  degrees  {hmatrixl  .phi_value  (}) 

«  ",  "  «  degrees  (hmatrixl . theta_value  ()) 

«  •* ,  "  «  degrees  (hmatrixl  .psi_value  ()) 

<<  " >  "  «  endl ; 

cout  «  "  hmatrix2  angles  =  <“  «  degrees  (hmat rix2 .phi_value  ()) 

<<  ",  "  <<  degrees  (hmat rix2 . theta_value  ()) 

<<  ",  "  «  degrees  (hmat rix2 .psi_value  {)) 

<<  ">  "  <<  endl; 

cour  "hmatrixl  =:  "  <<  hiTiatrixl  «  endl  ; 

//  two  alternate  methods  can  be  tested; 

hmatrixl  .  incremiental^rota t ion  (p,  q,  r,  delta_timie); 

hma tri>:2  ,  rota te  (p  *  deita_time,  q  *  delta__time,  r  *  delta_time)  ; 

cout  «  " - "  «  endl 

cin. get  (chcicei;  //  pause 

choice  =  '  * '  ;  II  duiTimy  value  to  print  keyboard  menu 
break ; 

ase  'D' :  case  'd' :  // - 

cout  «  "Defaults"  «  endl; 
cout  <■<  endl; 

vectorl  =  VectorBD  (); 

quaternionl  =  Quaternion  (); 

hmatrixl  =  Hmiatrix  (); 

cout  «  "Using  «  operators:"  «  endl; 

cout  «  "Default  vectorBD:  "  «  vectorl  «  endl ; 

cout  «  "Default  quaternion:  "  «  quaternionl  «  endl ; 

cout  «  "Default  Hmatrix:  "  «  hmatrixl; 

cout  <'<  "Using  print  methods:"  «  endl; 
cout  «  "Default  vectorBD:  "; 

vectorl .print  (); 

cout  «  endl  «  "Default  quaternion:  "; 

quaternionl .print  (); 

cout  «  endl  «  "Default  Hmatrix:  "; 

hmatrixl .print_hmatrix  (); 

cout  «  endl  ; 

cout  «  "initializations  &  assignment;"  «  endl; 
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vectorl 

= 

cout 

« 

quaternionl 

= 

cout 

<< 

hmatrixl 

cout 

Vector3D  ( 1 /  2  ,  3 )  ; 

"  VGCtor3D  (1,  2,  3}  ==  "  <<  vectorl  «  endl; 
Quaternion  (0,  0,  0); 

*•  Quaternion  (0,  0,  0)  ==  "  <<  quaternionl  «  endl; 
Hmatrix  (  4C,  50,  60, 

radians  (0),  radians  (-90),  radians  (0)  ); 
*'  Hmatrix  (  40,  50,  60,  radians  (0),  radians  (-90 “ 

-  radians  (0))  **  «  hmatrixl  <<  endl  ; 


hmatrixl 

couc 


=  Hmatrix  (Vector3D ( 40 , 50 , 60 ) , 

Vector3D (radians (0) ,  radians (-90 ) ,  radians (0 ))) ; 
«  "  Hmatrix  (Vector3D ( 40 ,  50,  60),” 

«  ”  Vector3D { radians  (0),  radians (-90 ) ,  radians (0)))  " 

«  hmatrixl  «  endl; 


quaternionl  =  Quaternion  (radians  (90),  0,  0)  * 

Quaternion  (0,  0,  radians  (90)); 

cout  «  "quaternion  mul tipi icaton  test  <90,0,0>  *  <0,0, 90>  =>"  <<  endl; 
cout  «  <"  «  degrees  (quaternionl  .phi_value  ())  «  ”/  " 

«  degrees  (quaternionl .  theta_value  ())  «  ”,  " 

degrees  (quaternionl  .psi_value  ())  «  “>  **  «  endl; 


r:gidoodyl  =  RigidBody  (); 

cout  endl; 

cout  <<  "  RigidBody  ()  defaults  printed  using  different  methods:” 
<<  rigidbodyl; 

rigidbody 1 , pr int_rigidbody  () ; 
cout  «  endl; 

disnetworkedrigidbody 1 . DISNetworkedRigidBody_initiali ze  ( ) ; 
cout  «  ”  DISNetworkedRigidBody  ()  defaults  printed  using  " 

«  "different  methods:"  «  disnetworkedrigidbody! ; 
di snetworkedrigidbody 1 . print_networkedrigidbody  ( )  ; 
cout  endl; 


cout  <<  "  UUVBody  ()  defaults  printed  using  different  methods;” 
uuvbcdyl; 

■juvbody  1  .  print_uuvbody  ( )  ; 

cfjcice  =  //  dummy  value  to  print  keyboard  menu 

break ; 


case  ' I ' :  case  ' i '  :  // - 

cout  "Invert  test:  uuvbody .  test_invert_matrix  ()”  «  endl  ; 
uuvbodyl . test_invert_matrix  ( ) ; 

ckoice  =  //  dummy  value  to  print  keyboard  menu 

break ; 


case  ' E' :  case  ' e' :  // - 

cout  «  ”dEad  reckoning  test”  «  endl; 

cout  «  "uuvbody .dead_reckon_test_with_execution_level  ():”  «  endl; 

uuvbody 1 . dead_reckon_test_with_execution_level  ( ) ; 

choice  =  //  dummy  value  to  print  keyboard  menu 

break; 

case  'T' :  case  't':  // - 

if  (TRACE  ==  0)  TRACE  =  1; 
else  TRACE  =  0; 
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cout  «  "Trace  toggled,  now  TRACE  =  “  «  TRACE  «  "  ( " ; 

if  (TRACE  0)  cout  «  "FALSE)"; 
else  cout  «  "TRUE)"; 

break; 

case  ' :  case  'p' :  If - 

cout  «  " PDU_skip_interval  change  (from  " 

«  uuvbodyl .  PDU_skip_intGrval_value  (}  «  " ) '*  «  endl  ; 

cout  <<  '' PDUs  will  only  be  sent  every  <value>  tenths  of  a  second." 

<<  endl; 

cout  «  "Values  less  than  10  may  make  applications  prone  to  crash." 

«  endl; 

cout  «  "Enter  new  value: 

int  new_skip_value; 
cun  >>  new_skip_value ; 
cout  <.<  endl; 

uuvbodyi  .  set_PDU_skip_interval  (new_skip_value}  ; 
m  e  d  k ; 

case  ' C ' :  case  ' c ' :  // - 

c  c-u  t  "Close"  < <^'  end  1  ; 

cout  «  ''DIS_net_cl ose  {)"  <*-"  endl; 

u u vbody  1 .  D I S_ne t_c lose  ( )  ; 

break ; 

cas:r  'Q'  :  case  'q'  :  // - 

cout  «  "Quit"  «  endl; 
dene  =  TRUE; 
b  r  e  a  K ; 

default:  // - 

if  {  !  isspace  (choice))  cout  <<  "  ?  unknown  option.."  «  endl; 
break ; 

}  //  end:  switch  (choice) 

}  while  (done  ==  FALSE) ; 

//  normal  exit 

} 

////// /////////////////////////////////////////////////////////////////// /////// 
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c. 


AUVglobals.h  Robot  Telemetry  Variables 


//////////////////////////////////////////////////////////////////////////////// 
Header  file:  AUVgiobals.H 

Aurhor:  Don  Brutzman 

Revised:  21  October  94 

Bysteui:  Irix  5.2 

Compiler:  ANSI  C*f  + 

Ccmp: iat ion :  irix>  make  dynamics 

r)escripr  ion :  Generalized  vehicle  state  variables,  customized  for 

NPS  AUV  II  operation  with  virtual  world  hydrodynamics  model 

Advisors:  Dr.  Mike  Zyda ,  Dr.  Bob  McGhee  and  Dr.  Tony  Healey 


References;  Brutzman,  Donald  P. ,  “Integrated  Simulation  for  Rapid 

Development  of  Autonomous  Underwater  Vehicles," 

IEEE  Oceanic  Engineering  Society  Autonomous  Underwater 
Vehicle  (AUV)  Conference  92,  Washington  DC,  June  4-5  1992, 
pp.  3-10. 

Brutzman,  Donald  P.,  "NPS  AUV  Integrated  Simulator," 

Masters  thesis.  Naval  Postgraduate  School,  Monterey  CA, 

March  1992. 

Healey,  Anthony  J.  and  Lienard,  David,  "Multivariable 
Sliding  Mode  Control  for  Autonomous  Diving  and  Steering 
of  Unmanned  Underwater  Vehicles,"  IEEE  Journal  of  Oceanic 
Engineering,  vol .  18  no.  3,  July  1993,  pp.  327-339, 

Lewis,  Edward  V.,  editor,  ^Principles  of  Naval 
Architecture  volumie  III_,  second  revision,  The  Society  of 
Naval  Architects  and  Marine  Engineers,  Jersey  City 
New  Jersey,  1988,  pp.  188-190  and  418-423. 

*/ 

//////////////////////,V/////////// ///////////////////////////////////////////// 
^  i  f  ndef  AWGLOBALSJ 

#define  AlRv'GLOBALS_H  //  prevent  errors  if  multiple  #includGs  present 

/****'^******i:*ir**iciric********i,*ic******-^ic***-x*-kir*****-)c**^*icictcic****t:****icitici,ir***icic/ 


/*  AUV  telemetry  state  vector  */ 
/*  */ 
/*  Note  these  are  globals  for  direct  access  by  any  world  model.  Refer  to  */ 
/*  individual  world  models  for  details.  Data  hiding  within  a  private  object  */ 
/*  is  not  necessary  since  all  values  are  transient  and  superseded  by  actual  */ 
/»  state  when  it  occurs.  Additionally,  half  of  the  state  variables  are  */ 
/*  provided  only  by  the  AUV  microprocessor  socket,  and  the  other  half  are  */ 
/*  provided  by  respective  world  models.  Thus  global  variables  in  this  design  */ 
/*  are  not  vulnerable  to  corruption  and  side  effects,  making  data  hiding  */ 
/*  unnecessary.  */ 
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AUV-provided  state  variables 


Static 

double 

AUV 

_time 

= 

0.0; 

// 

mission  time 

static 

double 

hlN 

.del ta_rudder 

= 

0.0; 

// 

positive  is  bow  rudder  to 

starboard 

static 

GGU  b I e 

AU\^ 

.del ta_planes 

0.0; 

// 

positive  is  bow  planes  to 

starboard 

3  r  a  t  i  c 

double 

AlTV^jjort^rprri 

C  .  0  ; 

if 

propeller  revolutions  per 

minute 

static 

double 

AUV. 

.stbd_rpiTi 

= 

0.0; 

// 

propeller  revolutions  per 

minute 

s  r  a  t  i  c 

double 

.bow_vertical 

0.0; 

// 

thruster  volts  24V 

=  3820 

rpm  no 

load 

static 

double 

AUV' 

.stern^vertica 1 

= 

0.0; 

// 

thruster  volts  24V 

=  3820 

rpm  no 

load 

static 

double 

AUV 

.bow_lateral 

= 

0.0; 

// 

thruster  volts  24V 

=  3820 

rpm  no 

load 

static 

double 

AUV. 

.stern_lateral 

= 

0.0; 

// 

thruster  volts  24V 

=  3820 

rpm  no 

load 

static 

double 

AUV^ 

.depth_cell 

= 

0.0; 

// 

pressure  sensor,  units  are  psid 

static 

double 

AU\^ 

.heading 

_ 

0.0; 

// 

gyrocompass  in  degrees 

static 

double 

AUV 

.roll_angle 

= 

0.0; 

// 

matches  intertial 

sensor 

onboard 

AUV 

static 

double 

AUV^. 

.pitch^angle 

0.0; 

// 

matches  intertial 

sensor 

onboard 

AUV 

static 

double 

AUV 

.roll_rate 

= 

0.0; 

// 

matches  intertial 

sensor 

onboard 

AUV 

static 

double 

AUV. 

jDi  tch_rate 

= 

0.0; 

// 

matches  intertial 

sensor 

onboard 

AUV 

static 

double 

AUV_yaw_rate 

= 

0.0; 

// 

matches  intertial 

sensor 

onboard 

AUV 

static 

int 

AlW 

.hour 

0; 

i  1 

internal  AUV  OS- 9 

system 

time,  unused 

static 

int 

AUV. 

.minute 

= 

0; 

static 

int 

AUV'. 

.second 

= 

0; 

Hydrodynamics-model-provided  state  variables 


static 

double 

AbTv  X 

_ 

O 

O 

// 

X 

position  in  world 

coordinates 

static 

double 

AUV'_y 

= 

0.0; 

// 

y 

position  in  world 

coordinates 

static 

double 

Abr^/_z 

= 

0.0; 

i  1 

z 

position  in  world 

coordinates 

static 

double 

AUV^phi 

= 

0.0; 

It 

roll 

posture  in  world 

coordinates 

s  t  a  1 1  c 

do^tl e 

AUv _ t  lie  t  a 

=: 

C  .  0 ; 

II 

pi  tch 

posture  in  world 

coordinates 

static 

G  G  u  b  1  e 

Abrv_psi 

= 

0.0; 

II 

yaw 

posture  in  world 

coordinates 

static 

double 

AUV_x_dot 

- 

0.0; 

II 

Euler  velocity  along  North-axis 

static 

double 

AW_y_dot 

= 

0.0; 

II 

Euler  velocity  along  East-axis 

static 

double 

ALtv_z_dot 

0.0; 

// 

Euler  velocity  along  Depth-axis 

s  t  a  L I  c 

double 

AU''^_jphi_dot 

0.0; 

// 

Euler 

rotation  rate  about  North-axis 

static 

double 

Atr^^_theta_dot 

= 

0.0; 

// 

Euler 

rotation  rate  about  East-axis 

static 

dou d1 e 

AtTv^^psi^dot 

= 

0.0; 

// 

Euler 

rotation  rate  about  Depth-axis 

s  a  I  c 

double 

AUV^_u 

— 

0.0; 

// 

surge 

linear  velocity 

along  x-axis 

static 

double 

AUV'_v 

= 

0.0; 

// 

sway 

linear  velocity 

along  y-axis 

static 

double 

AlTv^w 

= 

0.0; 

// 

heave 

linear  velocity 

along  x-axis 

static 

double 

AUV_^ 

= 

0.0; 

// 

roll 

angular  velocity 

about  x-axis 

static 

double 

AUV_q 

= 

0.0; 

// 

pi  tch 

angular  velocity 

about  y-axis 

static 

double 

AUV_r 

= 

0.0; 

// 

yaw 

angular  velocity 

about  z-axis 

static 

double 

AUV_u_dot 

0.0; 

// 

linear  acceleration 

along  x-axis 

static 

double 

AUV_v_dot 

= 

0.0; 

// 

linear  acceleration 

along  y-axis 

static 

double 

AUV_w_dot 

= 

0.0; 

// 

linear  acceleration 

along  x-axis 

static 

double 

AUV_p_dot 

= 

0.0; 

// 

angular  acceleration 

about  x-axis 

static 

double 

AUV  q_dot 

= 

0.0; 

// 

angular  acceleration 

about  y-axis 

static 

double 

AUV_r_dot 

= 

0.0; 

// 

angular  acceleration 

about  2-axis 

static 

double 

AUV_oceancurrent_ 

.u 

=: 

0.0; 

// 

Ocean  current  rate  along  North-axis 

static 

double 

AUV_oceancurrent. 

.v 

= 

0.0; 

// 

Ocean  current  rate  along  East-axis 

static 

double 

AUV_oceancurrent. 

= 

0.0; 

// 

Ocean  current  rate  along  Depth-axis 
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//  Prior  time  loop  saved  variables 


static  double  AUV_time_prior  =  0.0; 

static  double  AUV_x_prior  =  0.0; 
static  double  AUV_5/_prior  =  0.0; 
static  double  AUV_2_prior  =  0.0; 

static  double  AW_phi_prior  =  0.  Ob¬ 
stacle  double  AUV^thetajrior  =  0.  Ob- 
static  double  AUV_psi_prior  =  0.0; 


//  mission  time 

//  X  position  in  world  coordinates 
//  y  position  in  world  coordinates 
//  z  position  in  world  coordinates 

//  roll  posture  in  world  coordinates 
//  pitch  posture  in  world  coordinates 
//  yaw  posture  in  world  coordinates 


//  Sonar  model  provided  state  variables 


static 

double 

AUV 

STIOOO  bearing  = 

0.0; 

// 

ST_ 

1000 

conical 

pencil 

beam 

bearing 

static 

double 

AOT 

STIOOO  range  = 

0.0; 

// 

ST 

1000 

conical 

pencil 

beam 

range 

double 

AbT 

STi 00  0  St renQth= 

0.0; 

// 

ST_ 

1000 

conical 

pencil 

beam 

strength 

to::sr 

doubie 

AiJv 

_ST1 00C„x_cf  f set  = 

3.5; 

// 

ST 

_1000 

longitudinal  location  in  feet 

^  ^  ^ 

doubi e 

ATJ\^ 

ST725  bearing  = 

0.0; 

// 

ST 

725 

1  X  24 

sector 

beam 

bearing 

s  t  a  1 1  c 

double 

Au; 

_£T7 2 Strange  = 

0.0; 

/  / 

ST 

725 

1  X  24 

sector 

beam. 

range 

s  t  a  t  c 

doutle 

AUV 

_ST72  5__strength  = 

0.0; 

// 

ST_ 

725 

1  X  24 

sector 

beam 

strength 

const 

double 

AUV 

IST725_x_of fset  = 

3.5; 

// 

ST_ 

.725 

longitudinal  location  in  feet 

//  Mod. 

al-v;ide 

global  variables  and 

constants 

-  _  _ 

_  ^  - 

_  _  -  . 

-  -  - 
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D. 


UUVModeLh  Hydrodynamics  Model  Coefficients 


//////////////////////////////////////////////////////////////////////////////// 
/  ’■ 

Prograrr::  UUVmodel.H 

Author:  Don  Erutzman 


Revi sed : 


26  October  94 


Ey  stem: 

Coinpi  ler : 
Ccnpi I  at  ion : 


Debugging : 
Advi sors : 
References : 


Notes : 


Irix  5.2 
ANSI 

irix>  ir.ake  dynamics 
irix->  CC  UUVmodel.H  +w 

+w  ==  Warn  about  all  questionable  constructs. 

gravy  1 : --brut2man/dynamics»  lint  UUVmodel.h 

Dr.  Mike  Zyda,  Dr.  Bob  McGhee  and  Dr.  Tony  Healey 

Healey,  Anthony  J.  and  Lienard,  David,  “Multivariable 
Sliding  Mode  Control  for  Autonomous  Diving  and  Steering 
of  Unmanned  Underwater  Vehicles,”  IEEE  Journal  of  Oceanic 
Engineering,  vol.  16  no.  3,  July  1993,  pp .  327-339, 

Lewis,  Edward  V.,  editor,  ^Principles  of  Naval 
Architecture  volumie  III_,  second  revision.  The  Society  of 
Naval  Architects  and  Marine  Engineers,  Jersey  City 
New  Jersey,  1983,  pp.  188-190  and  418-423. 

Gertler,  Morton  and  Hagen,  Grant  R.,  ^Standard  Equations 
of  Motion  for  Submarine  Simulation_,  Naval  Ship 
Research  and  Development  Center  (NSRDC)  Research  and 
Development  Report  2510,  Washington  DC,  June  1967. 

Smith,  N.S.,  Crane  J.W.  and  Summey,  D.C.,  _SDV  Simulator 
Hydrodynamic  Coef f icients_,  Naval  Coastal  Systems  Center 
(NCSC),  Panama  City  Florida,  June  1978.  Declassified. 

Marco,  David.  “Slow  Speed  Control  and  Dynamic  Positioning 
of  an  Autonomous  Vehicle,"  Ph.D.  dissertation. 

Naval  Postgraduate  School,  Monterey  California,  March  1995. 

Bahrke,  Fredric  G.,  "On-Line  Identif icaton  of  the  Speed, 
Steering  and  Diving  Response  Parameters  of  an  Autonomous 
Underwater  Vehicle  from  Experimental  Data,"  Master's  Thesis, 
Naval  Postgraduate  School,  Monterey  California,  March  1992. 

Warner,  David  C.,  "Design,  Simulation  and  Experimental 
Verification  of  a  Computer  Model  and  Enhanced  Position 
Estimator  for  the  NPS  AUV  II,"  Master's  Thesis, 

Naval  Postgraduate  School,  Monterey  California,  December  1991. 

const  definitions  are  for  software  engineering  reliability 
they  can  be  changed  to  variables  if  coefficient  modification 
becomes  desirable 


*/ 

//////////////////////////////////////////////////////////////////////////////// 
#ifndef  UUVMODEL_H 

#define  UUVMODEL_H  //  prevent  errors  if  multiple  #includes  present 
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#Q€r f  i lit*  SI 


// 

// 


««  uncomment  this  statement  for  SI  units 
otherwise  standard  British  units  used 


// - // 

//  term  value  units  description 

;/ - // 

#ifdef  SI  //  Systeme  International  (metric)  units  - 


#ifdef  SI  //  Systeme  International  (metric)  units 


const 

double 

Weight 

// 

= 

435.0*0.454 

197.49 

// 

N 

Weight  (0.454  kg/lb  ==  1) 

const 

double 

Buoyancy 

// 

_ 

435.0*0.454 

197,49 

// 

N 

Buoyancy  (0.454  kg/lb  ==  1) 

const 

double 

L 

= 

2,23 

// 

m 

characteristic  length  87.625" 

const 

double 

9 

= 

9,81 

// 

m/s^2 

gravitational  constant 

const 

double 

rhc 

= 

1000 . 0 

// 

kg/in^3 

mass  density  of  fresh  water 

const 

double 

m 

// 

= 

Weight  /  g 
20.131498 

// 

N-s^2 /m 

vehicle  mass  incl.  free  flood 

^ de  f  1 1 

'je 

m4_f  t 

4 

(0.305) * {^P.: 

^05) 

// 

* (0.305) * 

(0.305)  //  (0,305  m/ft  ==  1) 

Inertia  matrix  coefficients 

const 

double 

I_x 

2 . 7  *m4_f 1 4 

// 

Nms^2 

=  I_xx  =  0.023364857 

const 

doL^bl  e 

1-V 

= 

2 . 7  *m4_f t4 

// 

NrTis^'2 

=  I_yy  =  0.023364857 

cons  t 

d  v.>u  e 

T 

= 

2.7*m4  ft4 

// 

NmiS'^2 

=  I_zz  =  0.023364857 

c  on  St 

d  ou  L  j.  e 

I-»xy 

= 

0.0 

i  1 

Nms-''2 

=  i_yx 

const 

doubl e 

I_xz 

= 

0.0 

// 

Nms^2 

=  I  zx 

const 

double 

= 

0.0 

// 

Nms'^2 

=  I^zy 

#ur:de 

f 

m4_f t 4 

// 

Centers  of  Gravity  &  Buoyancy 

const 

double 

X  G 

=  0.0 

;  // 

cm 

const 

double 

y-.G 

=  0.0 

;  // 

cm 

const 

double 

Z_G 

=  6.1 

;  // 

cm 

Note  CG  below  CB  Marco  0,5" 

const 

double 

X  & 

=  0.0 

;  // 

cm 

const 

doubl e 

y^B 

=  0.0 

;  // 

cm 

const 

d  c  u  b  J.  e 

2-B 

=  0.0 

;  // 

cm 

CB  at  center  of  UUV 

#el  se 

//  (not 

SI)  standard  British 

units  - 

const 

double 

Weight 

=  435.0 

;  // 

lb 

Weight 

const 

double 

Buoyancy 

=  435.0 

;  // 

lb 

Buoyancy 

const 

double 

L 

=  7.30208335 

;  // 

ft 

characteristic  length  87.625" 

const 

double 

Q 

=  32,174 

;  // 

f  t/s''2 

gravitational  constant 

const 

double 

rho 

=  1.94 

;  // 

slugs/f  t'^S 

mass  density  of  fresh  water 

const 

double 

m 

=  Weight  /  a 

;  // 

lb/ft-s^2 

vehicle  mass  incl.  free  flood 

// 

=  13.520234 

;  // 

Inertia  matrix  coefficients 

const 

double 

I  X 

=  2.7 

;  // 

Ib-f t-sec^ 

2ft^4  =I_xx 

const 

double 

=  42.0 

;  if 

f  t^4 

=i_yy 

const 

double 

1— z 

=  45.0 

;  // 

f  t^4 

=  I_zz 

const 

double 

l-.xy 

=  0.0 

;  // 

f  ^^4 

=  l_yx 

const 

double 

I  XZ 

=  0.0 

;  // 

f  t'"4 

=  I_ZX 

const 

double 

l^Z 

=  0.0 

;  // 

ft^4 

=  I_zy 

// 

Centers  of  Gravity  &  Buoyancy 

const 

double 

X  G 

=  0.125/12.0 

;  // 

ft 

0.010416667 

const 

double 

y-G 

=  0.0 

;  // 

ft 

const 

double 

Z_G 

=  1.07  /12.0 

;  // 

ft 

Note  CG  below  CB 

const 

double 

X  B 

=  0.125/12.0 

;  // 

ft 

0.010416667 

const 

double 

y_B 

=  0.0 

;  // 

ft 

const 

double 

z  B 

=  0.0 

;  // 

ft 

CB  at  center  of  UUV 

//  Thruster/propeller  distances  from  centerlines.  Note  stern/port  are  negative. 
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const 

double 

^bow_vertical 

1.41  ; 

// 

ft 

Marco 

17" 

measured 

13.0 

const 

double 

_stern_vertical  = 

-  1.41  ; 

// 

ft 

Marco  - 

17" 

measured  - 

21.0 

const 

double 

x_ 

_bow_lateral  = 

1.92  ; 

// 

ft 

Marco 

23“ 

measured 

19.0 

const 

doubl e 

x_ 

_stGrn_lateral  = 

-  1.92  ; 

// 

ft 

Marco  - 

23" 

measured  - 

26.0 

const 

double 

y_port_propel ler  = 

-  0.313; 

// 

ft 

Marco  - 

4“ 

measured  - 

3.75 

const 

double 

y. 

^stbd__propel  ler  = 

0.313; 

// 

ft 

Marco 

4" 

measured 

3.75 

#endi  f 


// - - // 

//  Surge  equation  of  motion  coefficients  // 


// - - // 

//  Surge  equation  of  motion  coefficients  // 


const 

doubl  e 

X  u  dot  = 

-2 .82E-3 

;  // 

Linear  force  coefficients  acting  in 

const 

doubl  e 

X  V  dot  = 

0.0 

;  // 

the  longitudinal  body  axis 

const 

double 

X  w  dot  = 

0.0 

;  // 

with  respect  to  subscripted 

const 

double 

X_p_dot  = 

0.0 

;  // 

motion  components 

const 

aoubie 

X__q_dot  = 

0.0 

;  // 

ccr;s: 

doubl  e 

X_i_dot  = 

0.0 

;  // 

const 

double 

X  uu  = 

0.0 

;  // 

const 

double 

X_vv 

0.0 

;  // 

old  -1.743E-2  SDV-9  holdover? 

const 

double 

X_ww  = 

0.0 

;  // 

const 

double 

X_pp 

0.0 

;  // 

const 

double 

X_gq 

0.0 

;  // 

c  on  s  L. 

double 

X_rr 

0.0 

;  // 

old  -7.53E-3  SDV-9  holdover? 

const 

double 

X_Drop  = 

7 .78E-3 

;  // 

X_prop  "constant"  no  longer 

appli 

cable 

const 

double 

X  rb  = 

0.283  * 

L;  // 

const 

double 

X_rs 

-0,377  ^ 

L;  // 

const 

doubl e 

X_uu_del  ta. 

_b__del  ta_b 

=  -1.018E-2  ;  //  drag  due  to  bow  plane 

const 

doubl e 

X_uu_del  ta. 

_s_del  ta_s 

=  -1.018E-2  ;  //  drag  due  to  stern  plane 

const 

double 

X_uu_dei  ta. 

_r_del  ta_r 

=  “l,018E-2  ;  //  drag  due  to  rudder 

const 

double 

X_pr 

0,0 

;  // 

(these  aren't  in  Bahrke  thesis  model 

const 

double 

X_wq  = 

0,0 

;  // 

const 

doubl e 

X_vp  = 

0.0 

;  // 

0  I  - 

double 

X_vr 

0.0 

;  // 

C  01  ]  s  t 

double 

X_uq_del  ta. 

_bow 

0.0 

;  // 

const 

double 

X_uq_del ta 

_stern  = 

0.0 

;  // 

const 

doubl e 

X_ur_del ta 

_rudder  = 

0.0 

;  // 

const 

double 

X_uv_del ta 

_rudder  = 

0.0 

;  // 

const 

double 

X_uw_del ta 

_bow  = 

0.0 

;  // 

const 

double 

X_uw_del ta 

_stern  = 

0.0 

;  // 

const 

double 

X_qdsn 

0.0 

;  //  no  longer  used  in  new  model 

const 

double 

X_wdsn 

= 

0.0 

;  //  no  longer  used  in  new  model 

const 

double 

X_dsdsn 

= 

0.0 

;  //  no  longer  used  in  new  model 

const 

double 

speed_per_ 

rpm  =2.0/ 

700.0 

;  //  steady  state:  0.0028571429 

//  =  (2.0  feet/sec)  per  700  rpm 

const 

double 

II 

o 

u 

0.00778 

;  // 

// 

// 


const 

double 

Y_u_dot  = 

0.0 

;  // 

Linear  force  coefficients  acting 

const 

double 

Y_v_dot  = 

-3 .43E-2 

;  // 

the  athwartships  body  axis 

const 

double 

Y_w_dot  = 

0.0 

;  // 

with  respect  to  subscripted 

const 

double 

Y_p_dot  = 

0.0 

;  // 

motion  components 

const 

double 

Y_q_dot  = 

0.0 

;  // 

const 

double 

Y_r_dot  = 

-1 .78E-3 

;  // 

// - 

//  Sway  equation  of  motion  coefficients 
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c  c  r*  ^ 

double 

Y_uu 

0.0 

// 

const 

double 

Y  uv  = 

-1.07E-1  ; 

// 

const 

double 

Y  uw 

0.0 

// 

const 

double 

Y_up 

0.0 

// 

const 

doubl  e 

Y  uq 

0.0 

// 

const 

double 

Y_ur 

0.0 

// 

Warner  =  1.187E-2;  Bahrke  =0.0 

const 

double 

Y__uu_del  ta_rb 

=  1.18E-2 

//  Marco  =  1.241E-2;  Bahrke  =  1.18E-2 

const 

double 

Y_uu_delta_rs 

=  1.18E-2 

//  Marco  =  1.241E-2;  Bahrke  =  1.18E-2 

const 

double 

Y_pq 

0.0 

// 

{these  aren't  in  Bahrke  thesis  model) 

const 

double 

Y  qr 

0.0 

// 

const 

double 

Y_vq 

0.0 

// 

const 

double 

Y_wp  = 

0.0  ; 

if 

const 

double 

Y_wr 

0.0 

// 

const 

double 

Y_vw  = 

0.0 

// 

const 

double 

C_dy  =: 

0.5 

// 

,  . 

_  _  _ _  _  _// 

// 

Heave  equation  of  motion 

coefficients  // 

c  ons: 

double 

Z  u  dot  = 

0.0 

// 

Linear  force  coefficients  acting  in 

const 

doubl e 

Z_v_dot  = 

0.0 

// 

the  vertical  body  axis 

const 

ocubl e 

Z_w_dct  = 

-9.34E-2  ; 

// 

with  respect  to  subscripted 

const 

double 

Z_p_dot  = 

0.0  ; 

if 

motion  components 

const 

doubl e 

Z  q_dot  = 

-2.53E-3  ; 

If 

C  OVi  s  t 

O  u  ^ 

Z_r_do!;  = 

0.0 

// 

cons: 

double 

Z_vv 

0.0 

// 

COT:  St 

o  o  u  b  j.  e 

Z _ u  w  = 

-7.e44E-i  ; 

if 

const 

double 

Z_up  = 

0.0 

if 

const 

double 

Z_uq  = 

0.0 

// 

Marco  =  -7.013E-2;  Bahrke  =  0.0 

const 

doubl e 

Z_rr  = 

0.0 

// 

const 

doubl e 

Z_pp  = 

0.0 

i  i 

const 

double 

Z  uu  delta  b  = 

-  2.11E-2 

// 

const 

d  ou  b  j.  e 

Z_uu_delca_s  = 

-  2.11E-2 

i 

// 

const 

doubl e 

Z_pr  = 

0.0 

if 

(these  aren^t  in  Bahrke  thesis  model) 

const 

doubl e 

Z_vp 

0.0 

// 

const 

double 

Z_vr  = 

0.0 

// 

ronst 

double 

Z_qn  = 

0.0 

// 

no  longer  used  in  new  model 

const 

double 

Z_wn 

0.0 

if 

no  longer  used  in  new  model 

const 

double 

Z_dsn  = 

0.0 

// 

no  longer  used  in  new  model 

const 

double 

C_dz 

0.6 

// 

// - 

— 

- // 

// 

Roll  equation  of  motion 

coefficients  // 

const 

double 

K  u  dot  = 

0.0 

// 

Angular  force  coefficient  acting 

const 

double 

K_v_dot  = 

0.0 

// 

about  the  longitudinal  body  axis 

const 

double 

K_w_dot  = 

0.0 

// 

with  respect  to  subscripted 

const 

double 

K_p_dot  = 

2.4E-4 

// 

motion  components 

const 

double 

K_g_dot  = 

0.0 

// 

const 

double 

K_r_dot  = 

0.0 

// 

const 

doubl G 

K_uu 

0.0 

// 

const 

double 

K_uv 

0.0 

// 

const 

double 

K_uw  = 

0.0 

if 

const 

double 

K_up 

5.4E-3 

if 

surge-related  roll  damping  drag 

const 

double 

K_uq  = 

0.0 

// 

const 

double 

K_ur 

0.0  ; 

// 

const 

double 

K_uu_planes  = 

0.0 

// 

(these  aren't  in  Bahrke  thesis  model) 
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const 

double 

K_pq 

=: 

0.0 

;  // 

const 

double 

K  qr 

= 

0.0 

;  // 

const 

double 

K_vq 

= 

0.0 

;  // 

const 

double 

K_wp 

0.0 

;  // 

const 

double 

K_wr 

= 

0.0 

;  U 

const 

double 

K_\"w 

= 

0.0 

;  /'  / 

const 

double 

K_prop 

0.0 

;  /  / 

K_j3rop  “constant"  no  longer  applicable 

const 

double 

K_pn 

0.0 

;  // 

no  longer  used  in  new  model 

const 

double 

Kjp 

-2 .02E-2 

;  // 

test  value  for  p-squared  damping 

// 

static  roll  damping  drag 

// 

100  *  Healey  minimal  estimate 

// 

(-2 .02E-4) 

const 

doubl e 

K_p)p/57 .3 

;  // 

estimate  based  on  quadratic  term 

// 

{K_pp)  equivalent  damping  at  1  deg /sec 

_____  _  .  /  / 

''  ‘ 

Pitch  equa 

--  /  / 

tion  of  motion  coefficients  // 

double 

M_u_dot 

0.0 

;  // 

Angular  force  coefficient  acting 

or  ~  t 

double 

M  V  dot 

= 

0.0 

;  // 

about  the  athwartships  body  axis 

COI'iSt 

double 

M_w_dot 

= 

-2 .53E-3 

;  // 

with  respect  to  subscripted 

C  0  i  *  s  t 

double 

C.O 

;  // 

motion  components 

const 

double 

M_q_dct 

= 

-6 .25E-3 

;  // 

const 

double 

PI_r_dot 

0.0 

;  // 

const 

double 

M_uu 

- 

0.0 

;  // 

const 

double 

M_vv 

= 

0.0 

;  // 

const 

double 

M_uw 

0.0 

;  // 

const 

double 

M_pp 

= 

0.0 

;  // 

const 

double 

M_rr 

0.0 

;  // 

const 

double 

M_uq 

= 

-1 . 53E-2 

;  // 

surge-related  pitch  damping  drag  *** 

cor:  St 

double 

M_uu_de. 

.  ta. 

_bow  =  -  0 

.283 

*  L  *  Z_uu„del ta_b; 

/  i 

note  (-}  Z  uu  delta  b 

//  =  0 

.043602433 

c  on  St 

double 

M  uu  delta 

_3tern  =  -»■  0 

.377 

*  L  »  Z_uu_del ta_s; 

// 

note  (-)  Z_uu  delta  s 

//  =  -  0 

.058085219 

const 

double 

M_pr 

- 

0.0 

;  // 

(these  aren't  in  Bahrke  thesis  model) 

const 

double 

K_vp 

0.0 

;  // 

const 

d  G  u  b  i  e 

M_vr 

= 

0.0 

;  // 

const 

double 

M^ijrop 

= 

0 .0 

;  // 

Mjrop  "constant"  no  longer  applicable 

const 

doubl  e 

M_qn 

=; 

0.0 

;  // 

no  longer  used  in  new  model 

const 

double 

M_wn 

= 

0.0 

;  // 

no  longer  used  in  new  model 

const 

double 

M_dsn 

= 

C.O 

;  // 

no  longer  used  in  new  model 

const 

double 

M_qq 

n 

-7 .OOE-3 

;  // 

slightly  larger  than  N_rr  estimate 

// 

test  value  for  q-squared 

// 

static  pitch  damping  drag 

// 

estimated  M_qq  -  K_pp  *  length  /  width 

// 

Torsiello  ~  0.005*  7.3'/  10.1*  =  .005 

const 

double 

M_q 

= 

M_qq  /  57.3; 

// 

estimate  based  on  quadratic  term 

// 

(M_qq)  equivalent  damping  at  1  deg/sec 

// - 

//  Yaw  equation  of  motion  coefficients  // 
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const 

double 

N_u_dot  = 

0.0 

• 

// 

Angular  force  coefficient 

acting 

const 

double 

N_v_dot  = 

-1 .78E-3 

; 

// 

about  the  vertical  body 

axis 

const 

double 

N_w_dot  = 

0.0 

; 

// 

with  respect  to  subscripted 

const 

double 

N_p_dot  = 

0.0 

; 

// 

motion  components 

cor:  St 

dou  bl e 

N_q_dot  = 

0.0 

/ 

If 

double 

N_r_dot  = 

-4  .7E-4 

/  i 

const 

double 

N_uu  = 

0.0 

// 

const 

double 

N_uv  = 

0.0 

; 

// 

Marco  =  -7.69E-3; 

const 

double 

N__uw  = 

0.0 

* 

if 

const 

double 

N_up  = 

0.0 

; 

// 

const 

double 

N  uq  =: 

0.0 

; 

// 

const 

double 

N  ur 

-3 .90E-3 

! 

// 

surge-related  yaw  damping  drag 

//  N_ 

uu_del  ta. 

_rb  and  N_uu_del ta_rs  not 

symmetric  due  to  different  moment  arms 

//  CO 

nst  double  N_uu_delta_ 

i-b  =  1.3259 

//  Marco 

//  const  double  N_uu_delta„ 

rs  =  1.7663 

//  Marco 

const 

double 

N_uu_delta_rb 

=  0.283 

* 

L  * 

Y_uu_delta_rb;  //  Bahrke 

=; 

0 . 024 

37762 

const 

double 

N_u  u_de  1 1  a_r  s 

=  0.377 

L  * 

Y_uu_del ta_rs;  //  Bahrke 

= 

0.03247476 

cor^i^.r  doutle  N_prop  =  0.0  ;  //  Normally  0.0  yaw  moment  due  to  paired 

//  counter-rotating  propellers; 

//  however  N_prop  is  not  zero  if  propel lor  rpms  are  independent 

.//  thus  yaw  equation  of  motion  now  has  yaw  moments  due  to  propellers 


/ 

'  and  N_prop  " 

constant 

”  i  s  no 

longer  applicable 

const 

double 

N_pq 

0.0 

;  // 

(these  aren't  in  Bahrke  thesis  model) 

c  on  St 

dou bl e 

= 

0.0 

;  // 

ccn.?t 

double 

N_vq 

0 . 0 

;  /  / 

const 

double 

= 

0 . 0 

;  if 

cor.  St 

double 

N__w'  r 

= 

0.0 

;  // 

const 

Q  C  u  b  r  e 

N_vw^ 

= 

0.0 

;  // 

c  on  St 

double 

N_rr 

-5 .48E- 

3  ;  // 

// 
// 
// 
// 
// 
// 

Torsiello  value  p.ll3  adjusted  for 
correction;  static  yaw  damping  drag 
estimated  N_rr  M_qq  *  height/  width 

=  .040  *  10.1"  /  16.5" 
=  0.005 

Torsiello:  0.005 

Healey:  N_rr  ~  M_qq 

//  al 
/  / 

ternate 

N_rr  = 

2*2# 

using 

*  1.92' 
r_max  = 

/(rho/2  *  r_max  *  r_max)  =>  0.0048473244 

16  deg/sec  Torsiello  which  is  consistent 

const 

double 

N_r 

=  N 

_rr  /  57 

.3;  // 

// 

estimate  based  on  quadratic  term 
(N_rr)  equivalent  damping  at  1  deg/sec 

// - u 


//  from  Dave  Marco's  dynamics  model: 

if  DEFINE  THE  LENGTH  X,  BREADTH  BR,  AND  HEIGHT  HH  TERMS 

double  XX  [15]  =  { 

-43.9/12.0, 

-39.2/12.0, 

-35.2/12.0, 

-31.2/12.0, 

-27.2/12.0, 

-10.0/12.0, 

0.0/12.0, 

10.0/12 .0, 

26.8/12.0, 

32.0/12.0, 
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37.8/12.0, 

40.8/12.0, 

42.3/12.0, 

43.3/12.0, 

43.7/12.0); 

double  hh  [15] 
0.0/12.0, 
2.7/12  .0, 
5.2/12.0, 

7  .  6/12 . 0 , 

10. 1/12.0, 
10.1/12.0, 

1 C  .  1  / 1 2 . 0 , 


10.1/12.0, 

9.6/12.0, 

7 . 6/12 .0, 


bb  [15 


16-5/12 

16.5/12 


9.5/12.0, 

7.0/12.0, 

4 .0/12 .0, 

0 . 0  /■  i  2 . 0  }  j 


UUVMODEL  H 
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UUVBody.C  Unmanned  Underwater  Vehicle  Networked  Rigid  Body 


'  / ,  ■ , ,  •'  /  /  /  / 
Prog  rani : 
Description : 

Revi sed: 

Sy  s  t  em : 
Compiler : 
Compilation : 


Advisors : 


References : 


'•/////////////// //// //////////////////////////////// /////////// 
UUVBody . C 

Six  degree-of - f reedom  underwater  vehicle  hydrodynamics 
based  on  Healey  model 

28  October  94 

Irix  5.2 

ANSI  C++ 

irix>  make  dynamics 

irix>  CC  UUVBody.C  -Im  -c  -g  +w 

-c  ==  Produce  binaries  only/  suppressing  the  link  phase. 

+w  ==  Warn  about  all  questionable  constructs. 
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Equations  of  motion  tested  satisfactorily, 
verification  against  in-water  tests  remains. 
Move  utilities  to  math_utilities . c 


Future  work: 


Comments  and  suggestions  are  welcome! 


iiiiuiniiiiiiuuiiiiimiiiniuiiiiiiiiiiiiiiiuiiiiiiiiiiiiniuiiniiiiiii 


#ifndef  UUVBODY_C 
# define  UUVBODY_C 

char  *  DISCLIENT  = 


//  prevent  errors  if  multiple  #includes  present 
^/DlS.mcast/src/client  -r  & " ; 
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4* include  "SonarMndel .  C" 

# include  "UUVmodel . H" 

# include  “DISNetworkedRigidBody .C " 

//////////////////////////////////////////////////////////////////////////////// 

class  UuVBody  :  public  DISNecworkedRigidBody 
{ 

p  r 1 va  t  e : 

Hiriatrix  Hprevious  ; 

/ /  member  data  fields 


doubl e 

current_uuv_ 

t  ime 

double 

U; 

// 

Linear  &  angular  velocities 

dcul:  1  e 

V; 

// 

in  body  coordinates 

double 

W; 

// 

(note:  RigidBody  is  world  coordinates 

d(.-U  J..-  u  tr 

P; 

doubi e 

Q } 

double 

R; 

doubl e 

u_dot ; 

// 

Linear  &  angular  accelerations 

double 

V_dot ; 

// 

in  body  coordinates 

double 

w _ d  O  t  ; 

G  c  u  r  ^  e 

p_do  t ; 

double 

'IL-do  ^  /■ 

double 

r_do  t  ; 

do'^ble 

X_dO  t ; 

// 

Euler  velocities,  world  coordinates 

doubl  tr 

y_do  t ; 

double 

2_do  t ; 

double 

phi_dct ; 

// 

Euler  rotation  rates,  world  coordinates 

double 

theta_dot ; 

double 

psi_dot ; 

double 

mass  ( 6 1  [ 6 ]  ; 

// 

mass  matrix  acceleration  coefficients 

double 

mass_inverse 

(6]  1 

[  6  ]  ;  /  / 

mass  matrix  inverted 

double 

rh  s  [  6  ]  ; 

// 

right-hand-sides  of  equations  of  motion 

double 

new_acceleration 

16];  // 

product  [mass_inverse] [rhs] 

double 

new_veloci cy 

(6] 

^  // 

(averaged_accelerations  *  dt ) 

/  / 

+  oid_velocities 

//  AW 

telemetry  state 

vector  is  located  in  "AUV  qlobals-h"  file 

public : 

//  member  constructor  and  destructor  functions 


UUVBody 

^UUVBody 

0  ; 

0  { 

/*  null  body  */ 

} 

//  operators 

void  multiply6x6_6 

{double 

left_matrix 

(6]  [6]  , 

double 

right_inatrix 

[6], 

double 

result_matrix 

[6]  )  ; 
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void 

mul tiply6x6_6x6 

(double  left_matrix  [6] [6 
double  right_matrix  [6] [6 
double  result_matrix  [6] [6 

friend 

ostream^  operator  << 

(ostream&c  out,  UUVBody&  uuvb) 

inspect 

ion  methods 

void 

pri nt_uuvbody 

0  ; 

void 

print_mat rixb 

(double  output_matrix  [6]); 

void 

print_mat rix6x6 

(double  output_jnatrix  [6]  [6]) 

double 

current_uuv_time_value 

0  const; 

double 

u_dot_value 

0  const; 

double 

v_do revalue 

0  const; 

double 

w_dot_value 

0  const; 

double 

p_dot_value 

(;  const; 

double 

q_dot_value 

()  const; 

double 

r_dot_value 

0  const; 

double 

x_dot_value 

0  const; 

double 

y_-dot_value 

{)  const; 

double 

z_dot_value 

0  const; 

modi fy i 

ng  methods 

void 

UU\"3  0  dy  _i  n  i  t  i  a  1  i  2  e 

0  ; 

void 

set_current_uuv_t ime 

(double  new_current_uuv_time) 

void 

set_accel era t ions 

(double  new_u_dot, 

double  new_v_dot, 
double  new_w_doc, 
doubl e  new_p_dot , 
double  new_q_dot, 
double  new_r_dot) ; 

void  set_l  inear_acceleracions  (double  new__u_dot, 

double  new_v_dot, 
double  new_w_dot} ; 

void  set_angular_accelerations  (double  new__p_dot, 

double  new_g__dot, 
double  new_r_dot); 

//  vehicle  socket  communications  with  dynamics 


void 

void 

loop_test_wich_execution_level  () ; 

//  vehicle  socket  communications  with  no  i 
dea  d_reckon_t  es  t_wi th_execu  t i on_l evel 

dynamics 

0  ; 

void 

swap 

(double 

*  a,  double  *  : 

double 

square 

(double 

value) ; 

double 

nonzerosign 

(double 

value) ; 

void 

clamp 

(double 

*  clampee, 

double 

void 

epsilon 

i n ve  r  t_ma  s  s_ma  t  r i x 

double 

double 

char 

0  ; 

0  ; 

absolute_jnin 
absolute_max 
*  name ) ; 
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void 

test_invert_mat rix 

0 

void 

caiculate_mass_matrix 

0 

void 

i n  t  eg  ra  t  e_equ  a  t i on  e_o  f _mo  t i on 

0 

//////////////////////////////////////////////////////////////////////////////// 


//  constructor  methods 


nirv^Body :  :  UUVBody  (}  //  default  constructor 

i 

Hp  r e V  i  ou  s  =  Hm a  t  r  i  x  f )  ; 


RigidBody_ini t ialize  (); 


Di SNerworkedRigi dBody_i 

curr^nt_uuv_time  =  0.0; 

U  0 . 0  ; 

V  =0.0; 

=0.0; 

?  =0.0; 

Q  =  C  .  0  ; 

R  =0.0; 


w_dot 

p_dot 

q_dot 

r_dot 

x_dot 

y_dot 

2_dct 

ph:_dot 

theta_dc 

psi^dot 


initialize  (}; 


//  inherited  method 
//  inherited  method 


int  index  =  0;  index  <=  5;  index++)  rhs  [index]  = 


ca 1 cu: a t  e_ma2 
invert  rriass  n 


^matrix  (); 
trix  ( }  ; 


operators 


ostreamc^  operator  «  (ostream^  out,  UUVBody^  uuvb) 

{ 

int  row,  col ; 

uuvb .print_networkedrigidbody  (} ; 

out  <<  ’‘current_uuv_time  =  "  «  uuvb . cur rent_uuv_time  «  endl ; 

out  «  ''<u_dot,  v_dot,  w_dot,  p_dot,  q_dot,  r_dot>  =  "  «  "<" 

«*  uuvb.u_dot  «  “,  "  «  uuvb.v_dot  «  ",  "  «  uuvb.w_dot  «  ", 
<'r  uuvb.p_dot  <<  " ,  "  «  uuvb.q_dot  <<  " ,  "  «  uuvb.r_dot  <<  **>" 
«  endl ; 


"Mass  matrix:"  «  endl; 
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0 ;  row 


5 ;  row  +  + ) 
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// 


// 


void  UUVBody : :  print_uuvbody  () 
pr  irit_ner.workedrigidbody  ( }  ; 


cout 

" cur rent_uuv 

_time  = 

" 

<< 

current_uuv_time  <<  endl ; 

cout 

<< 

"<U,  U,  W,  P 

,  Q,  R>  body 

=  ■■ 

«  "<•* 

<< 

U  «  " ,  "  « 

V  «  “,  “ 

<< 

w 

<< 

F  «  "  ,  “  « 

Q  «  “,  * 

« 

R 

<<  *>"  <<  endl; 

cout 

<< 

'‘<u_dot,  v_dot ,  w_dot, 

P_' 

dot 

,  q_dot,  r_dot>  body  =  *  « 

« 

u_dot  <<  " , 

"  «  v_dot 

<< 

"  ^ 

"  «  w_dot  «  " ,  " 

p_dot  «  " , 

"  «  q^dot 

« 

11 

"  <<  r_dot  «  «  endl; 

Vector3E'  euler_posi tion_rates  =  Vector3D  (x_dot,  y_dot,  z_dot}; 

cour.  <<  *’<x_dot,  z^doo  =  “  «  euler_posi tion_rates  «  endl; 

Vecnor3D  euler_angul ar_rat€S  =  Vector3D  (phi_dot,  theta^dot,  psi_dot) ; 
cout  "■:phi_dot,  theta^dot,  psi_dot>  =  “  «  euler_angular_rates  «  endl; 

cour  “Mass  matrix:  “  endl; 

prinr_rr-a:  r  ix6>:n  fmassj  ; 

cout  "Mass  inverse  matrix:"  <<  end!  ; 

print^ma trix6x6  (mass_inverse} ; 

cout  <"<  "Right-hand  side  (RHS)  of  equations  of  motion: 
print_ma trix6  (rhs); 

!  / - // 

void  UUVBodv : :  print_ma t r ix6  (double  output_matrix  [6]) 

( 

int  row; 


} 

// 


} 

cou 


;rcw  =  0;  row  <=  5;  row  -)-+) 

cout  <'<'  output_matrix  [row]; 
if  (row  <  5}  cout  «  ",  "  ; 

■'  ]  ”  <<  endl  ; 


void  TJUVBody::  print_matrix6x6  (double  output_matrix  [6] [6]) 


// 


row,  ccl  ; 

{ row  =  0  ; 

row^  <=  5;  row  ++) 

cout  " 

[ " ; 

for  (col 

=  0;  col  <=  5;  col 

+  +  ) 

{ 

cout 

«  output_matrix  1 

[row 

]  Icol]  ; 

i  ^ 

(col  <  5)  cout  « 

"  / 

else 

cout  « 

H  j  U 

«  endl 

} 

// - // 


double  UUVBody::  current_uuv_time_value  () 
const 
{ 

return  current_uuv_time; 
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// 


double  ITTjVBody::  u_dot_value  () 

const 

V 

return  u_dot ; 

) 


double  Uia^Body::  v__doc_value  () 
const 

return  v_dot ; 


d c u  n I e  UUVE  ody : :  w_do  t _va 1 u  e  ( ) 

const 

i 

return  w_dot; 

} 

// - 

double  Uli^CBody::  p_dot_value  (} 
const 

return  p_dot; 


dounle  •J'.'VBody :  :  q_dot_value  (; 


i 

return  q_dct; 

// - 

double  UU\'Bcdy :  :  r_dot_value  () 


return  r_dot; 


d  Gu  b  1  e  Ub" ^’5  ody  :  :  x_do  t_va  1  u  e  ( ) 
const 
{ 

returr:  x_dot; 

} 

// - 

double  UUVBody::  y_dot_value  (} 
const 
{ 

return  y_dot; 

} 

// - 

double  UUb'Body :  :  z_dot_value  () 
const 
{ 

return  2_dot; 

} 

// - 

//  modifying  methods 
// - 


// 


// 


// 


// 


// 


// 


// 

// 
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void  UUVBcdy : :  UUVBody_ini tiali ze  (} 

{ 

RigidEcdy_initialize  {); 
DISNecworkedRigidBody_ini tiali ze  () ; 


currerjt_uuv_i;iine  =  0.0; 

U  =0.0; 
V  =0.0; 
W  =0.0; 
P  =0.0; 
Q  =0.0; 
R  =0.0; 


0.0; 

0.0; 

0.0; 

0.0; 

0.0; 

0.0; 

=  0.0; 

=  0 . 0  ; 

=  0.0; 

=  C  .  0  ; 
dec  =  0.0; 

C  =  C)  .  0  ; 

for  i. inc  index  =  0;  index  <=  5;  indexT  +  )  rhs  [index]  =  0.0; 

cal  culace_mass_iriat  rix  (); 
inverc_rcass_ina trix  i )  ; 

bearing  =  0.0;  //  ST_1000  conical  pencil  beam  bearing 

range  =  0.0;  //  ST_1000  conical  pencil  beam  range 

screngcli=  0.0;  //  ST_1000  conical  pencil  beam  strength 

5_bearing  =  0.0;  //  ST_72  5  1  x  24  sector  beam  bearing 

Airv_£T /2 5_ra]‘ige  =  0.0;  //  ST_72  5  1  x  24  sector  bearci  range 

AW_ST72  5_strength  =  0.0;  //  ST_725  1  x  24  sector  beam  strength 

AUV_bow_vercical  =  0.0;  //  thruster  volts  24V  =  3820  rpm  no  load 

Ab^/_stern_vertical  =  0.0;  //  thruster  volts  24V  =  3820  rpm  no  load 

AW_bow_laceral  =  0.0;  //  thruster  volts  24V  =  3820  rpm  no  load 

Air7_stern_lateral  =  0.0;  //  thruster  volts  24V  =  3820  rpm  no  load 

AlJv_del  ta_ rudder  =0.0;  //  positive  is  bow  rudder  to  starboard 

AbV_deI ta_planes  =  0.0;  //  positive  is  bow  planes  to  starboard 

AU\'_jjort_rpm  =  0.0;  //  propeller  revolutions  per  minute 

AUV_stbd_rpm  =  0.0;  //  propeller  revolutions  per  minute 

) 

// - 

void  UUVBody::  set_current_uuv_time  (double  new_current_uuv_t ime) 
current_uuv_time  =  new_current_uuv  time; 

) 

- - 


AuO^STlOOO 

AljV_ST1000 

auv_st:oog 


x_doc 
y_dot 
2  doc 


psi_ac 


u_dot 

v_dcc 

w_doL 

p_dcc 

CLdoc 

r_doc 


//  inherited  method 
//  inherited  method 


void  UUVBody::  set_accelerations 
{ 

u_dot  =  new_u_dot; 
v_d  o  t  =  n  e  w_  v_d  o  t  ; 
w_do  t  =  new_w_do  t  ; 


(double  new_u__dot ,  double  new_v_dot, 
double  new_w__dot ,  double  new  p  dot/ 
double  new_q_dot,  double  new_r_dot) 


p_dor  =  new_p__dot; 

=  new_q_dot; 
i_doc  =  new^rjot; 


void  ITltv^Body  :  :  set_l inear_accelerat ions 

i 

u_dot  =  new_u_dot; 
v_dot  =  new_v_dot; 
w_d  0 1  =  11  e  v;_w_d  o  t  ; 

) 

(double 

double 

new_u_dot , 
new_w__do  t ) 

double 

new_v_dot , 

void  lJT_r\/Eody :  :  set__angular_accelerations 

p_d  or  =  n  e w_p_do  t ; 
o_dot  =  new_q__dot; 
i_dot  =  new_r_dot; 

/. _ _  _ 

(double 

double 

new_p_dot , 
new_r_dot ) 

double 

new_q_dot. 

//  vehicle  socket  communications  tests 

void  UlT/Body : :  loop_test_wi th_execution_leveI  (} 

int  read_f roiri_socket_result  =  0; 
iViZ  wrice_!:o_sockeL_resuIt  =  0; 

//  print_uuvbod>’  (};  //  diagnostic 

:r  :TRACE;  coct  «.-<  "  i  ioop_test_with_execution_level  start]"  «  endl; 
oper._execurion_level_sccket  ();  //  repeated  calls  should  not  reopen  it 

ccut  <<  endl ; 

ccur  <<  "To  listen  on  default  multicast  port  (on  another  machine):  " 

•'  endl ; 

cou:  DIE'lLIEKT  —  endl; 

cout  *•'<  endl; 

cout  "ElS_net_open  ( }  :  "  «  endl; 

L:lE_net_open  ( ;  ; 

while  (TRUE;  //  loop  until  break 

torri_socket_result  =  read__f rom_execution_level_socket  {); 

if  { read_f rom_socket_result  ==  -4)  //  time/position/orientation  received 

.  1 

cout  «  "position  or  orientation  command  received:  <" 

«  AUV_time  «  ">  <“ 

«  AUV_x  «  " ,  "  «  AUV_y  «  “ ,  "  «  AUV_z  «  "  >  < " 

«  AUV_phi  «  "  «  AUV_theta  «  ",  "  «  AUV__psi  «  ">• 

«  endl; 

RigidBody_initialize  {)  ; 

Vector3D  vectorl  =  Vector3D  (AUV_x,  AUV_y,  AUV_z); 

hmatrix  =  Hmatrix  (vectorl, 

radians  (AUV_phi ) , 
radians  {AUV_theta) , 
radians  (AUV_j)si)); 
hmatrix .print_hmatrix  ( ) ; 
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Hpi'tvious  =  hmatrix; 

) 

else  if  ( read_f rorri_socket_resul t  ==  TRUE)  //  valid  report  received 
integrate_equations_of_motion  ( ) ; 

if  (DIS_net_wri te  (}  ==  FALSE)  break;  //  send  out  PDU,  otherwise  done 
test_tarik_sonar_model  ();  //  parallelize  &  generalize  the  sonar  model 

wri te_to_executi on_level__socket  ();  //  send  back  full  telemetry 


if  ( ( read_f rom_socket_resul t  ==  -3)  j  I  //  KILL  signal  was  received 
( read_f rom_socket_resul t  --  -4)  )  //  position  signal  was  received 

{ 

ccut  «  "KILL/position/orientation  signal  received,  "  «  endl ; 


cout  <<  "freeze 

the  AW  where 

it 

is."  «  endl ; 

Au''v^_U 

0.0; 

// 

surge  linear  velocity 

along  x-axis 

AUV_v 

= 

0.0; 

// 

sway  linear  velocity 

along  y-axis 

AW^_w 

0.0; 

// 

heave  linear  velocity 

along  x-axis 

AW  p 

= 

0 . 0  ; 

// 

roll  angular  velocity 

about  x-axis 

AUV_q 

= 

0.0; 

// 

pitch  angular  velocity 

about  y-axis 

AW_r 

= 

0.0; 

// 

yaw  angular  velocity 

about  z-axis 

AWWt'_dot 

= 

0 . 0  ; 

// 

linear  acceleration 

along  x-axis 

AW_v_dot 

0.0; 

t  i 

linear  acceleration 

along  y-axis 

AW_w_dot 

= 

0.0; 

// 

linear  acceleration 

along  x-axis 

AhF\/'_jj_dot 

0.0; 

// 

angular  acceleration 

about  x-axis 

ALTvWii^dot 

= 

0 . 0  ; 

// 

angular  acceleration 

about  y-axis 

AU\'_r_dot 

= 

0.0; 

// 

angular  acceleration 

about  z-axis 

AUV_x_dot 

= 

0.0; 

// 

Euler  velocity  alo 

mg  North-axis 

AW^_dot 

0.0; 

// 

Euler  velocity  alona  East-axis 

ALAWz_dot 

_ 

0.0; 

it 

Euler  velocity  alo 

ng  Depth-axis 

AW_phi_dot 

= 

0.0; 

if 

Euler  rotation  rate  about  North-axis 

AW_theta_dot 

= 

0.0; 

II 

Euler  rotation  rate  abo 

ut  East-axis 

AW_psi_dot 

= 

0.0; 

II 

Euler  rotation  rate  abo 

ut  Depth-axis 

clock_t  start_busywai t_ciock  =  clock  (); 

while  (clock  ()  <  start_busywait_clock  +  1.0  *  CLOCKS_PER_SEC} 

{ 

/*  busy  wait  */ 

} 

wri te_to_socket__resul t  =  DIS_net_write  {); 

cour  "freeze  AW  PDU  sent:  "  «  wri  te_to_socket_resul  t  «  endl  ; 


if  ( read_f rom_socket_resul t  ==  -3)  //  KILL  signal  was  received 

i 

cout  «  "KILL  signal  received,  exit  " 

<<  "UUVBody . loop_test_wi th_execution_level "  «  endl ; 

if  (TRACE)  cout  <<  " [ loop_test_with_execution_level  complete]* 

«  endl ; 

return;  //  all  done 

) 

) 

//  unreachable  exit  point 

//  shutdown_socket  ()  must  have  been  invoked  already  to  exit  preceding  loop 
}  //  loop_test_with_execution_level  complete 

// - n 

void  UUVBody::  dead_reckon_test_with_execution_level  () 
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int  r€ad_f rorTi^sockGt^resuI t  =  0; 
int  write_to_socket:_resulc  =  0; 

cout  <■<■  endl  ; 

co^t  "To  listen  on  default  multicast  port:  "  «  endl ; 
cout  DISCLIENT  «  endl  ; 

cout  endl  ; 

op<^n_execution_level_socket  (); 

cout  "DIS__net__opGn  ()"  «  endl  ; 

DI£__net_open  (  )  ; 

w :\iie  ( TR u E )  / /  loop  until  break 

read_f rom_£ocket_resul t  =  read^f rom_execution_l€vel_socket  (}; 

if  (  ( read_f  roirf_socket_rGsul  t  ==  -2)  II  if  shutdown  signal  was  received 
^  ( reader  rorri_socket_resul t  ==  -3))  //  quit  signal  was  received 

cout  "shutdown  signal  received,  freeze  the  AUV  where  it  is" 
endl; 


docket  start_busywait_clock  =  clock  {}; 

while  (clock  ()  <  start_busywai t_clock  +  1.0  *  CLOCKS_PER_SEC) 
/•*■  busy  wait  */ 


wri te_to_socket_resul t  =  DIS_net_wri te  (); 

cout  «  "freeze  AUV  PDU  sent:  *'  <<  write_to_socket_resul t  «  endl; 

if  {read_from_socket_result  ==  -3)  //  quit  signal  was  received 

cout  «  "quit  signal  received,  freeze  the  AUV  where  it  is"  «  endl; 
cout  <<  "  and  rezero  for  another  loop  if  necessairy"  «  endl; 

UUVBody_initialize  {); 

) 

break; 


//  equations  of  motion  not  integrated,  execution  level  does  dead  reckoning 
if  (DIS_net_write  {)  ==  FALSE)  break;  //  send  out  a  PDU,  otherwise  done 
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write_co_execut ion_level_socket  ();  //  note  state  vector  preserved 

} 

If  shutdown_socket  ()  must  have  been  invoked  already  to  exit  preceding  loop 

//  couc  " DIS_net_close  {)"  «  endl  ; 

//  DIS_net_close  (); 


// - // 

//  vehicle  dynamics  functions 

// - n 

void  UU\^Body::  swan  (double  *  a,  double  *  b) 

( 

double  temp  =  *  a; 

*  a  =  ^  b; 

■*  b  =  temp; 

) 

// - // 

double  I'nj'/Eody::  square  (double  value) 

{ 


return  fvalue  *  value) ; 

} 

/ / - // 

double  irjtvBody::  nonzerosign  (double  value} 

if  (value  0.0)  return  sign  (value); 
else  return  +  1.0; 


void  lAiVBody::  damp  (double  *  clampee,  double  absolute_min , 

double  absolute_max,  char  *  name) 

I 

double  new_value,  local_min,  local_max; 

:f  : 'absclute^max  ==  0.0)  &&  ( ab30lute_min  ==  0.0))  return;  //  no  clamp 

if  {absolute_max  >=  absolute_min)  //  ensure  min  Sc  max  used  in  proper  order 
i 

local_min  =  absolute_min ; 
local_max  =  absolu te_max ; 

} 

el  se 
{ 

local_min  =  absolute_max ; 
local_max  =  absolute^min; 

if  (  clampee)  >  local_max) 
i 

new_value  =  local_max; 

cout  <<  "clamping  "  «  name  «  "  from  " 

<<  *  clampee  «  "  to  "  «  new_value  «  endl; 

*  clampee  =  new_value; 

) 

if  (  (*  clampee)  <  local_min) 

{ 

new_value  =  local_min; 

cout  <:<:  "clamping  "  «  name  «  "  from  " 

<•"  *  clampee  «  "  to  "  «  new__value  «  endl; 

*  clampee  =  new_value; 
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} 

)  ! - u 

double  UUVBody::  epsilon  {)  //  not  used  in  revised  surge  EOM  «<««««««« 

//  due  CO  problems  in  analytic  derivation 
//  retained  for  archival  purposes 

double  average_rpm  =  {AUV_porc_rpm  +  AUV__stbd_rpm)  /  2.0; 

double  n  =  average_rpm; 

double  eta ; 


^ta  =  sp 

u'^rpm 

*  n  /  nonzerosign  i 

[  U  ); 

cout 

"eta 

—  M 

eta  endl  ; 

double  C 

f  — 

_L.  — 

0.008 

*  L*b  *  eta  *  fabs 

( eta ) 

/ 

2.0 

double  C 

= 

0.008 

*  L*L 

/ 

2.0 

double  result; 

' ■  X^prop  redefinition:  net  pushing  force  on  vehicle,  account  for  water  flow 
/.'  X_prcr-  =  C_d0  *  {eta  *  fabs  (eta)  -  1.0); 

//  X_prop  =  C_d0  *  (eta  *  fabs  (eta)); 

//  ecu:  -  <■  "X_prcp  =  "  «  X__prop  «  endl; 

result  ^  -1.0  +  (sign  (n)  /  nonzerosign  (  U  }) 

*  (sqrt  (C„t  -  1.0}  -  1.0)  /  (sqrt  (C_tl  +  1.0)  -  1.0); 

//  kill  this  function  due  to  rewritten  X_propulsion  terms 

//  cout  •:<  "epsilon  ()  =  "  «  result  «  endl; 
return  result; 


// 


veld  tr-ody :  :  invert_iriass_matrix  ()  //  adapted  from  Numerical  Recipes  in  C 

//  2nd  edition  pp.  39-40 


double 

double 


indxc  [6],  indxr  [6],  ipiv  [6]; 
1 ,  : ,  k ,  row,  col ,  1 ,  11; 
b i g ,  dummy ,  p i v i n v ; 
miass_copy  [  6  ]  [  6  ]  ; 


for  (j  =  0;  j  <=  5;  j++) 

{ 

mass_copy  [i](j]  =  mass  [i][j]; 

if  (i  ==  j)  mass_inverse  [iJlj]  =  1.0; 

else  mass_inverse  [i]lj]  =  0.0; 

) 


) 

//  int  arrays  ipiv,  indxr  and  indxc  are  used  for  bookkeeping  on  the  pivoting 

for  (j  =  0;  j  <=  5;  j++)  ipiv  [j]  =  0;  //  initialize  loop 

for  (i  =  0;  i  <=  5;  i++)  //  main  loop  over  columns  reduced 

{ 

big  =  0.0; 

for  (j  =  0;  j  <=  5;  j++)  //  outer  loop  of  pivot  search 

{ 
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if  (ipiv  [jl  !=  1) 
i 

for  (k  =  0;  k  <=  5;  k++) 

{ 

if  (ipiv  [kj  ==  0) 

{ 

if  (fabs  imass^copy  [j][k])  >=  big) 

big  =  fabs  (mass^copy  Ij][k]); 
row  =  j ; 
col  =  k; 


else  if  (ipiv  [k]  >  1) 

cout  <<  “Error:  singular  mass  matrix"  «  endl; 


( ipiv  [col ] ) ; 

//  see  detailed  comments  in  reference  code  for  pivot  bookkeeping  scheme 


for  (1  =:  0;  1  <=  5;  1  +  +  )  swap  (&  mass_copy  [row]  [1], 

&  mass_copy  icol][l]); 

for  (1  =  0;  1  <=  5;  1++)  swap  (&  mass_inverse  [row][l], 

Sc  mass_inverse  [colJ[l]}; 

) 

inoxr  [i]  =  row;  //  we  are  now  ready  to  divide  pivot  row  by  pivot  element 
jnaxc  li]  =  col;  //  which  is  located  at  [row,  col] 

It  (iTiass_copy  [col]  [col]  ==  0.0) 


cout  << 

"invert 

error, 

pi  vi 

nv  =  i . 0 

/  mass_ 

copy  [ 

riia  s  s 

_copy  [ col ] [ col j 

=  1.0 

f  or 

( i  =0  ; 

1  <=  5 

;  1--^ 

for 

(1  =0; 

1  <=  5 

;  1  +  + 

for 

(11  =  0; 

11  <=  5 

;  11  +  + 

\ 

if  (11 

!=  col) 

mass_copy 


[col ] [ 1 ]  *=  pivinv; 


(11  !=  col)  //  except  for  the  pivot  row,  of  course 

dummy  =  mass_copy  [11] [col]; 
mass_copy  [11] [col]  =  0.0; 
for  (1  =  0;  1  <=  5;  1  +  4.) 

^ass_copy  [11]  [1]  -=  mass__copy  [col][l]  *  dummy; 

for  (1  =  0;  1  <=  5;  1++) 

mass_inverse  [11] [1]  -=  mass_inverse  [col][l]  *  dummy; 


r 

//  we  now  unscramble  the  columns  by  interchanging  in  reverse  order 


for  (1  -  5;  1  >=  0;  1--) 

{ 

if  (indxr  [1]  !=  indxc  [1]) 

{ 

for  (k  =  0;  k  <=  5;  k++) 


swap  (&  mass_copy  [k] [indxr  [1]],  &  mass_copy  [k] [indxc  [1]]); 
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- - 

void  UUVBody :  :  test_invert_inatrix  () 

{ 

i n  c  i ,  i; 

c  s  -  c  u  a.  a  c  fa*  iTiB  s  s  rn a  c.  i*  i  x  ( )  ) 

cout  «'  "Original  mass  matrix:'*  «  endl; 
pi-int_mat:rix6x6  (mass)  ; 

irivert_mass_matrix  (); 

couc  *>-■  endl  ; 

cour  .  "Inverted  mass  matrix:"  <<  endl; 
print_matrix6x6  (mass_inverse) ; 

{ 

for  (j  =  0;  i  <=  5;  j++) 

rr,ass  [iMj;  =  mass_inverse  iij'ji; 

; 

ir:vert_mass_matr  ix  {); 

cout  ''  '■  ei'icil  /‘  -u  1  4- 

couc  "Double  invert_mass_matrix  0  should  get  back  to 
<■<•  "original  mass  matrix:"  «  endl  ; 

print_ma trix6x6  {mass__inverse}  ; 

calcul ate_mass_macrix  ();  //  restore 

invert_mass_ma trix  (); 

} 


--// 


---// 


void 

body  :  : 

calculate. 

_mia  s  s. 

..matrix 

0 

4idet  i  ne 

UDOT 

0 

// 

matrix 

indices 

oe  line 

VDOT 

1 

define 

WDOT 

2 

^def i ne 

FOOT 

3 

«def : ne 

C)tOT 

4 

f  I  Virr 

P.  1  OT 

U 

*def ine 

SURGE 

0 

»» de  tine 

SWAY 

1 

4idef  ine 

HEAVE 

2 

#def ine 

ROLL 

3 

4»def  ine 

PITCH 

4 

#def ine 

YAW 

S 

mass 

[SURGE] [UDOT]  = 

m 

-  0.5 

*■ 

rho  L*L*L 

*  X_u_dot 

mass 

[SURGE] [VDOT]  = 

0.0; 

ma  s  s 

[SURGE 

[WDOT]  = 

0.0; 

mass 

[SURGE] [PDOT]  = 

0.0; 

mass 

[SURGE] [QDOT]  = 

m  * 

(  z_G); 

mass 

[SURGE] [RDOT]  = 

m  * 

{-y_G)  ; 

mass 

[SWAY 

] [UDOT]  = 

0.0; 

mass 

[SWAY 

]  [VDOT]  = 

m 

o 

1 

* 

rho  *  L*L*L 

*  Y_y_dot 

mass 

[SWAY 

] (WDOT)  = 

0.0; 

mass 

[SWAY 

]  [PDOT]  = 

m  * 

{-2_G) 

-  0.5 

* 

rho  *  L*L*L*L 

*  Y_p_dot 

mass 

[SWAY 

]  [QDOT]  = 

0.0; 

mass 

[SWAY 

] [ROOT]  = 

m  * 

{  x_G) 

-  0.5 

* 

rho  *  L*L*L*L 

*  Y_r_dot 

mass 

[HEAVE] [UDOT]  = 

O 

O 
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mass 

(HEAVE] [VDOT] 

= 

0.0; 

mass 

(HEAVE] (WDOT) 

= 

m 

-  0.5 

* 

rho 

* 

h*h*L 

* 

Z  w  dot; 

mass 

[HEAVE] [PDOT] 

m  * 

V  G)  ; 

ma  s  s 

[HEAVE] [QDOT] 

= 

m  * 

-X_G)  -  0.5 

* 

rho 

* 

L*L*L*L 

ic 

Z_q_dot ; 

mass 

[HEAVE 

[RDOT] 

0 . 0 ; 

mass 

[ROLL 

[UDOT] 

0.0; 

miass 

[ROLL 

[VDOT] 

= 

m  * 

-Z  G)  -  0.5 

* 

rho 

*• 

L*L’^L*L 

ic 

K  V  dot; 

mass 

[ROLL 

[WDOT] 

= 

m  * 

y  G)  ; 

mass 

[ROLL 

[PDOT] 

= 

I_x 

-  0.5 

* 

rho 

* 

L*L*L*L*L 

■k 

K_p_dot ; 

mass 

[ROLL 

[QDOT] 

= 

“ I_xv ; 

mass 

[ROLL  ] [ROOT] 

-  I_XZ 

-  0.5 

★ 

rho 

★ 

L*L*L*L*L 

* 

K_r_dot ; 

mass 

[PITCH] [UDOT] 

- 

m  *  ( 

Z-G)  ; 

ma  ss 

[PITCH] [VDOT] 

= 

0.0; 

mass 

[ PITCH] [WDOT] 

m  *  { 

-X_G)  -  0.5 

★ 

rho 

★ 

L*L*L*L 

* 

M_w_dot ; 

mass 

[PITCH] [PDOT] 

= 

-I  xy; 

mass 

[PITCH] [QDOT] 

= 

i-y 

-  0.5 

* 

rho 

* 

L*L*L*L*L 

* 

M_q_dot ; 

mass 

[PITCH] [ROOT] 

= 

mass 

[YAW  1 [UDOT] 

= 

m  *  ( 

-y^G;  ; 

mass 

[  YAW 

[VDOT] 

= 

m  *  ( 

X_G }  -0.5 

* 

rho 

•k 

L*L*L*L 

* 

N_v_dot ; 

mass 

[YAW 

[WDOT] 

= 

C  .  0  ; 

mass 

[  YAW  i 

[PDOT] 

= 

-I_xz 

-  0.5 

* 

rho 

* 

L*L*L*L*L 

* 

N_p_dot  ; 

mass 

[YAW  j 

[QDOT] 

= 

-1^2; 

m.ass 

[YAW  ] 

[RDOT] 

-  0.5 

★ 

rho 

■k 

L*L*L*L*L 

* 

N_r__dot ; 

1  \v.p:- 

Eody  :  : 

Integra 

te. 

_equa  t 

1  ons_of_motion 

0 

c’jrrenc_uuv_cime  =  AW^time; 
dc'jL^e  dt  =  current_u’Liv  time 


time_of_posture_value  () 


//  miss: 


:lock  was  reset,  rezero  the  dynamics  model 


current_uuv_time  =  AW_time; 
set_time_of_posture  (AUV'_time}  ; 

3ec_velcci ties  (0.0,  0.0,  0.0,  0.0,  0.0,  0.0); 

set_accelerations  (0.0,  0.0,  0.0,  0.0,  0.0,  0.0  ; 
dt  =  0.0; 

U  =0.0; 

V  =0,0; 

W  =  0 . 0  ; 

P  =0.0; 

Q  =0.0; 

R  =0.0; 


double  rho2 
double  L2 
double  L3 
double  L4 
double  L5 


=  rho  /  2.0; 
=  L*L; 

=  L*L*L; 

=  L  *  L  *  L  ’ 
=  L  *  L  *  L  ’ 


//  note  that  sign  is  not  preserved  in  the  following  squared  variables 

//  in  order  to  present  consistent  naming  with  Healey  reference  paper 

//  To  preserve  sign,  use  {U  *  fabs  (U) }  etc. 

double  P2  =  p  *  P; 

double  Q2  =  Q  *  Q; 

double  R2  =  R  *  R; 

double  U2  =  U  *  U; 

double  V2  =  V  *  V; 

double  W2  =  W  *  W; 
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//  calculate  world  coordinate  posture  rates,  use  holding  variables  for  speed 

double  PHI  =  phi_value  (); 

double  THETA  =  theta_value  {); 

double  PSI  ==  psi_value  (); 

double  sinPHI  =  sin  (  PHI  ) ; 
double  cosPHI  =  cos  (  PHI  ) ; 
double  sinTHETA  =  sin  (  THETA  ) ; 
double  cosTHETA  =  cos  (  THETA  ); 
double  sinPSI  =  sin  (  PSI  } ; 
douDle  cosPSI  =  cos  (  PSI  ) ; 

double  EPSILON  =  epsilon  {);  //  no  longer  used  in  revised  model 

double  del  ta_i:)  lane  system  =  AW^delta^planes; 
do u ole  del ta_p lane s_bow  -  -  AUV_del ta_p lanes ; 

double  del ta_rudder_stern  =  AUV_delta_rudder ; 
double  del ta_rudder_bow  =  -  AUV_del ta_rudder ; 

//  irit^grate  drag  forces  over  the  vehicle  -  --  --  --  --  --  --  --  - 


double 

3way_integral 

double 

h  e  a  V  e  _  i  n  t  e  g  r  a  1 

dcuoT  e 

pi tch_integral 

douole 

y aw_integral 

dOUO_  ■c- 

dx,  U_cf_>:; 

int  X  index  =  0;  x_index  <  14;  x_index  ++)  //  longitudinal  centerline 
dx  =  fabs  {XX  [x_index]  -  xx  [x_index  •+■  1]}; 

U_cf_x  =  sqrt  (  square  (V  +  xx  [x_index]  *  R) 

+  square  (W  -  xx  [x_index]  *  Q) ) ; 
if  {U„cf_x  >  l.OE-6)  //  arbitrary'  small  non-0  minimum 
^  sway^integral  e=  rho2  »  (  C_dy  *  hh  [x^index] 

*  square  { (V  +  xx  [x_index]  *  R)) 

+  C_dz  *  bb  [x_index] 

*  square  ( (W  -  xx  (x_index]  *  Q) ) ) 
»  (V  +  XX  [x_index]  *  R)  *  dx  /  U_cf„x; 

heave_integral  +=  rho2  *  (  C_dy  *  hh  [x_index] 

*  square  { (V  +  xx  [x_index]  *  R) ) 

+  C_dz  *  bb  [x_index] 

*  square  { (W  -  xx  [x_index]  *  Q) ) ) 
*  (W  -  XX  [x_index]  ♦  Q)  *  dx  /  U_cf_x; 

pitch_incegral  +=  rho2  *  {  C_dy  *  hh  [x_index] 

»  square  ( (V  +  xx  [x_index]  *  R)) 

+  C_dz  *  bb  [x_index] 
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*  square  { (W  -  xx  [x_index]  *  Q) ) ) 

*  {W  -  XX  [x_index]  *  Q) 

//  note  sign  correction 

*  XX  [x_index]  *  dx  /  U_cf_x; 

yaw_intGgral  +=  rho2  *  (  C_dy  *  hh  [x_index] 

*  square  ( (V  +  xx  (x_index]  *  R) ) 

+  C_d2  *  bb  [x_index] 

*  square  ( (W  -  xx  [x_index]  *  Q) ) ) 

*  (V  +  XX  [x_index]  *  R) 

*  XX  [x_index]  *  dx  /  U_cf_x; 


if  (TRACE) 

cout  <<  "dx  =  '•  <<  dx  <<  ",  U_cf_x  =  "  «  U_cf_x 

<<  ",  sway_integral  =  "  «  sway_integral  «  endl  ; 

cout  "dx  =  "  «  dx  <<  ",  U^cf^x  =  "  «  U_cf_x 

<<  ",  heave_intGgral  =  "  «  hGave_integral  <<  endl ; 

cout  <■<  "dx  =  "  <<  dx  <<  ",  U_cf_x  =  "  «  U_cf_x 

«•  ",  pi tch^integral  =  "  «  pi tch_integral  «  endl ; 

cout  <<-•  "dx  -  "  <^<'  dx  <<'  ",  U_cf_x  =  "  «  U_cf_x 

",  yaw_integrai  =  "  <<  yaw_integral  <<  endl  ; 


calcuiate  Equations  of  Motion  right-hand  sides  // 

/■  /  /  //  .■  i  !  i  i  !  i  1 1 !  I  i  i  i  !  i  II 1 1 !  1 1 !  I ;  I  i  !  Ill  1 1 1 !  i  !  1 1  / 1  n  !  1 1 !  I  i  1 1  i  I  fj  I  i  i  I !  j  !  I  f  1 1 !  i  I !  n 

rns  i  SURGE]  =  //  Surge  Motion  Equation  right  hand  side - // 


m  *  f  (V  *  R) 

-  {W  * 

Q) 

+  X 

_G  -  {Q2 

+  R2)  -  y_( 

rho2  *  L4  * 

(  Xjp 

* 

P2 

+  X  qq  * 

Q2 

+  X_rr 

* 

R2 

+  X_pr  * 

P*R) 

rho2  *  L3  * 

(  X_wq 

★ 

W*Q 

+  X_vp.  * 

V*P  +  X_vr 

+  U*Q  *  (  X_uq_del ta_bow 

+  X_uq__del  ta_stern 


del ta_planes_bow 
del ta_pl anes_s tern) 


+  U*R 

) 


X_ur__del  ta_rudder  *  del ta_rudder_bow 
X_u  r_de 1 1  a_r udde  r  ^  de 1 1  a_r udde  r_s  tern) 


+  rho2  *  L2  *  (  X_w  *  V2  +  X_ww  *  W2 


+  U*V  *  (  X_uv__delta_r udder  *  delta_rudder_stern) 

+  U*W  *  (  X__uw_delta_bow  *  del  ta_planes_bow 

+  X_uw_delta_stern  *  delta_planes_stern) 

+  U  *  fabs  (U)  *  {  X_uu_delta_b_del  ta_b 

*  delta_planes_bow 
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delta_planes_bow 
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*  delta_planes_bow 

*  delta_planes_bow 

+  X_uu_del  ta_s_delta_s 

*  del ta_planes_stern 

*  del ta_planes__s tern 

+  X_uu_delta_r_del  ta_r 

*  delta_rudder_bow 

*  delta_rudder_bow 

+  X_uu_delta_r_del  ta_r 

*  del ta_rudder_s tern 

*  del ta_rudder_s tern) 


''termS^''  «  -  (Weight  -  Buoyancy)  *  sinTHETA 
end  1  ; 

*' termb ,  terTTi7=  "  «  "EPSILON  terms,  no  longer  used" 
endl  ; 

<<  *'terrri6="  <<  rho2  *  L3  X_qdsn  *  U*Q  *  del ta_planes_stern 
*  EPSILON  «  endl; 

«  "terrTi7="  «  rho2  *  L2  *  EPSILON  *  (  X_wdsn  *  U*W 

*  del ta_planes_s tern 

+  X_dsdsn  *  U2  *  del ta__planes_s tern 

*  del ta_planes_s tern) 

<<  endl ; 

"termc="  *■:<■  +  rho2  *  L2  *  C_dO  *  square  (speed_per_rpin) 

^  0.5  *  (  A'J\''_i:=ort_rpiTi  *  fabs  {AUV_port_rpin) 

+  AUV_s t bd_r pm  *  fabs  (AUV_stbd_rpin)  ) 


"teriTi9  = 

"  < 

•:  - 

rho2 

;  *  L2 

■at 

C\ 

_dC  *  U 

*  fabs  (U) 

*■<  endl; 

/  /  /  .  /•//////- 

/  '  '' 

/  // 

/  /  /  /  / 

/  /  /  /  / 

//. 

// 

/////// 

////// 

///, 

///////, 

rhs  [SWAY  ] 

= 

// 

Sway 

Moti 

on 

Equation 

right 

hand  side 

m  *  ( - 

{U 

* 

R)  + 

(W  * 

P) 

- 

X_G  * 

(P  *  Q 

) 

+ 

Y-G  * 

(P2  + 

R2) 

- 

Z_G  * 

(Q  * 

R)  ) 

+  rho2  * 

L4 

(  Y_pq 

* 

P 

*Q  + 

Y_qr 

* 

Q*R) 

+  rho2  * 

L3 

* 

( 

up 

* 

U 

*P  + 

Y_ur 

* 

U*R 

+  Y_ 

yq 

* 

V 

*Q  + 

Y_wp 

* 

W*P 

+  rho2  * 

L2 

* 

{  Y_ 

uv 

* 

u 

*V  + 

Y_vw 

* 

V*W 

+  U*fabs(U)  *  Y_uu_delta_rb  *  del ta_rudder_bow 
+  U*fabs(U)  *  Y_uu_delta_rs  *  del ta_rudder_s tern) 


_inteqral 
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+  (Weight  -  Buoyancy)  *  cosTHETA  *  sinPHI 

-  (2.0  /  (24.0  »  24.0))  //  each  thruster  2.0  lb  per  24V  signal  squared 

*  (  AW_bow_lateral  *  tabs  (AUV_bow_lateral ) 

+  AUV_stern_lateral  *  fabs  (AUV_stern_lateral) ) ; 


V'//////////////////////// 

,  f  (TRACED  //  Eway  TRACE 


ecu:: 

«  " *  sway 

terml= " 

«  It  *  (*-  (U 

*  R)  +  (W  * 

P) 

-  X_G 

*  (P  *  Q) 

+  Y— G 

*  (P2  4-  R2) 

- 

*  (Q  *  R)  ) 

cout 

*'■  "teLirC-' 
1"  d  i  ' 

*  4 

rhc2  *  L4  *  ( 

Y_pq  *  P 

*Q  + 

Y_qr 

*  Q*R) 

ecu  t 

..  11  t-  ,-p-  "3  1 

utrirr;U  = 

'  <'v'  4 

rho2  *  L3  *  ( 

Y_up  *  U 

*P  + 

Y_ur 

»  U*R 

<-'  endl ; 

4 

Y_^vq  *  V-Q 

+  Y_wp 

*  W*P 

4  Y_wr  *  w*: 

cout 

‘'terTri4=' 

'  4 

rho2  *  L2  *  ( 

Y_uv  * 

u*v 

4  Y_vw 

*  V*W 

U*fabs(U)  *  Y_ 

uu_del  ta_rb 

*  delta, 

_r udder 

_bow 

<;<'  endi  ; 

U*fabs(U)  *  Y_ 

uu_delta_rs 

*  delta, 

_r udder 

_stern) 

cout 

«  "term5=' 
«■  endi ; 

■  -c< 

sway_integral 

cout 

"teiTri6=’ 
endi  ; 

'  «  4 

(Weight  -  Buoyancy)  *  cosTHETA  * 

sinPHI 

(■'O'  ’  ^ 

<<■  “reririT^' 

•  ...  - 

{2.0  /  (24.0 

*  24.0}) 

//  each  thruster  2.0  lb  per  24V  signal  squared 

*  {  AUV_bow_lateral  *  fabs  (AUV_bow_lateral ) 

+  AUV_stern_lateral  *  fabs  (AUV_stern_lateral ) ) 


rhs  i HEAVE]  =  //  Heave  Motion  Equation  right  hand  side 


m  *  1 

(  (U  *  Q) 

-  (V  * 

P) 

-  X_G  *  (P  *  R) 

-  y^G  *  (Q  ^ 

R) 

4  2_G  *  {P2  4 

Q2)  ) 

4  rho2 

*  L4  *  ( 

Z__PP 

★ 

P2  4  Z_pr  * 

P*R  4  Z_rr 

*  R2) 

4  rho2 

*  L3  *  ( 

Z_uq 

* 

U*Q  +  Z_vp  * 

V*P  4  Z_vr 

*  V*R) 

4  rho2 

*  L2  *  ( 

Z_uw 

★ 

U*W  +  Z_w  * 

V2 

+  {  U*fabs(U)  *  Z_uu_delta_b  *  delta_planes_bow  ) 

+  (  U*fabs{U)  *  Z_uu_delta_s  *  d€lta_planes_stern) ) 
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-  heave_integral 

+  (Weight  -  Buoyancy)  *  cosTHETA  *  cosPHI 

//  EPEILON  termc  have  been  removed  due  to  revised  equations  of  motion 


//  rho2  *  L3  * 

Z_qn 

*  U*Q  *  EPS 

/ /  ^  rho2  *  L2  * 

(  Z_wn 

*  u*w 

// 

+  Z_dsn 

*  U*fabs(U) 

+  (2.0  /  (24.0  *  24.0)}  //  each  thruster  2.0  lb  per  24V  signal  squared 

*  (  AIjV_bow_vertical  *  fabs  {AUV_bow_vertical )  + 

AW_stern_vertical  *  fabs  (AUV_stern_vertical )  )  ; 


//////////////////////// 


if  (TRACE)  //  Heave  TRACE 


C/ //////// //////////////// /////////// 


:out  heave  teniil="  <<  m  *  (  (U  *  Q)  -  {V  *  P)  -  x_G  *  (P  *  R) 

-  y_G  *  (Q  *  R) 

+  ZJ3  *  ( P2  +  Q2 ) ) 

<<  endl ; 


cout  « 

" term2= " 
Z_rr  * 

<< 

R2  ; 

<< 

rho2  *  L4  *  ( 
endl  ; 

Z_PP 

*  P2 

+  Z_pr  *  P*R 

cout 

"  term3= " 

-  Z_vr  * 

V*Ri 

<< 

rho2  *  L3  *  { 

:  endl ; 

Z_uq 

*  U*Q 

+  Z_vp  *  V*P 

cout  << 

"  terTri4=  " 

<< 

rho2  L2  *  ( 

Z_uw 

*  U*W 

+  Z_vv  *  V2 

+ 

(  fabs (U 1  * 

Z_uu_ 

_delta_b  * 

del ta_planes_bow  } 

endl ; 

- 

(  U*fabs(U)  * 

Z_uu^ 

_delta_s  * 

del ta_planes_stern) 

cout  <<' 

<  <, 

"  term 5=  *' 
endii  ; 

« 

- 

heave_integral 

cout  «' 

"  teiTr!6= " 

<•< 

(Weight  -  Buoya 

ncy) 

*  cosTHET 

A  *  cosPHI 

t  e  rm /  = 
end!  ; 


"EPSILON  terms,  no  longer  used" 


cout  “term8="  «  +  rho2  ^  L2  *  (  Z_wn  *  U*W  ) 

«'  endl ; 

cout  «  “term9=‘‘  «  +  (2.0  /  (24.0  *  24.0)} 

//  each  thruster  2.0  lb  per  24V  signal  squared 


endl  ; 


*  (  AUV_bow_vertical  *  fabs  {AUV_bow_vertical )  + 

AUV_stern_vertical  *  fabs  (AUV_stern_vertical )  ) 


rhs  [ROLL  J  =  //  Roll  Motion  Equation  right  hand  side  • 

-  (I_z  -  I^)  *  Q*R  -  I_xy  *  P*R  +  I^z  *  (Q2  -  R2 ) 

-  m  -  (  y_G  *  (  ‘U*Q  +  V*P)  -  z_G  *  (  U*R  -  W*P) } 

+  rho2  L5  *  (  K_pq  *  P*Q  +  K__qr  *  Q*R 


I_xz  *  P*Q 
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+  K_pp  *  P  *  fabs(P) 

+  K_p  *  P  )  //  hovering  roll  drag 

-  rho2  *  L4  *  {  K_up  *  fabs(U)*P  +  K_ur  *  U*R  +  K_vq  *  V*Q 

+  K_wp  ^  W*F  +  K_wr  *  W*R) 

+  rho2  *  L3  *  (  K_uv  *  U*V  +  K_vw  ^  V*W 

-  U*fabs(U)  *  0.5  *  (  K_uu_planes  *  del ta_p lane s_bow 

+  K_uu_planes  *  delta jlanes_stern;  ) 
//  expected:  opposed  plane  directions  ''  cause  negation  &  cancellation 

+  (y_G  *  Weight  -  y_B  *  Buoyancy)  *  cosTHETA  *  cosPHI 

-  (z_G  *  Weight  -  z_B  *  Buoyancy)  *  cosTHETA  *  sinPHI; 

//  EPSILON  terms  have  been  removed  due  to  revised  equations  of  motion 
//  +  rho2  *  L4  *  K_pn  *  U*P  *  EPSILON 

.  rho:  ’  Li  ’  U*fabs(U)  *  K_p'rop;  //  oversimplified,  in  error 

/,  /,  /  : ,  ; ,  !  I !  i  :  i :  !  < :  !  i  !  n  i  !  I !  I  i  !  I !  I !  !  H  !  I !  I !  I !  I !  I !  I !  I !  I !  !  ! '  '  "  ’  "  !  '  '  "  "  '  '  '  "  ' 

if  f TRACE,  //  Roll  TRACE 

.....  ...  .oil  terml  =  "  «  -  (I_z  -  I_y)  *  Q*K  -  I_xy  P*R  +  I_yz  *  (Q2  -  R2) 

+  I_xz  P*Q 

•  endl  ; 

cout  <-  "term2=”  «  -  m  *  (  y_G  *  (  -U»Q  +  V*P)  -  z_G  *  (  U*R  -  W*P) ) 

<■<  endl  ; 


cout  «  ■■term3=‘'  «  +  rho2  *  L5 


K_pq  *  P*Q  +  K_qr  *  Q*R 


^  K_pp  *  F  *  fabs(P)  ^ 

+  K_p  *  F  )  //  hovering  roll  drag 

<<  endl; 

:out  <■:  “tenn4="  «  .  rho2  *  L4  ’  (  K_up  *  fabs(U)*P  +  K_ur  *  U*R 

+  K_vq  *  V*Q  +  K_wp  *  W*P  +  K_wr  *  W*R) 

endl ; 

cout  "term5="  «  .  rho2  *  L3  *  (  K_uv  *  U»V  +  K_vw  *  V*W 

-  U’-fabslU)  *  0.5  ♦  (  K_uu_planes  *  delta_planes_bow 

+  K_uu_planes  *  delta_planes_stern) 
//  expected:  opposed  plane  directions  cause  negation  &  cancellation 

«  end! ; 


cout 

"terTn6="  <^'  + 
endl ; 

(y-.G 

*  Weight  - 

y_B  * 

Buoyancy) 

*  cosTHETA  *  cosPHI 

cout 

<^<r 

« 

“term7="  - 

endl  ; 

(z_G 

*  Weight  - 

2_B  * 

Buoyancy) 

*  cosTHETA  *  sinPHI 

cout 

V  V 

V  V 

-terms,  term9='' 
endl ; 

« 

-EPSILON 

terms. 

no  longer 

used* 

//  cout  «  ■term8=*  «  +  rho2  *  L4  *  K_pn  *  U*P  *  EPSILON 
//  «  endl ; 

H  cout  «  ■term9=-  «  +  rho2  *  L3  *  U*fabs(U)  *  K_prop 
//  «  endl; 


rhs  [PITCH]  =  //  Pitch  Motion  Equation  right  hand  side 


V///// 
- // 


-  1^2)  * 

F’^R  +  I_xy 

Q’H  -  l-_/2  * 

P*Q  -  I_xz  *  (P2  -  R2) 

m  *  {  x_G  * 

(  -U*Q  +  V*Pi 

-  Z_G  *  {  - 

V*R  4  W*Q} ) 

rho2  *  L5  * 

(  M_^p  *  P2 

+  M_pr  *  P*: 

R  +  M_rr  *  R*fabs  (R) 

+  M_q  *  Q 
+  M__qq  *  Q  * 

fabs {Qj )  / / 

hovering  pitch  drag 

rho2  *  L4  * 

{  M_uq  *  U*Q 

4  M_vp  *  V*P 

4  M_vr  *  V*R) 

rhc2  ’  L3  * 

(  M_uw  *  U*W 

4  M_vv  *  V2 

+  U*fabs(U)  *  (  M_uu_del ta_bow  *  del ta_p lane s_bow 

+  M_uu_del ta_stern  *  delta_planes_stern) ) 

+  pi tch_integral  //  note  sign  corrections  to  Healey  pitch_integral 

-  (x_G  *  Weight  -  x_B  *  Buoyancy)  *  cosTHETA  *  cosPHI 

-  {z_G  *  Weight  -  z_B  *  Buoyancy)  *  sinTHETA 

^  {2.0  /  (24.0  *  24.0))  //  each  thruster  2.0  lb  per  24V  signal  squared 

f !  multiplied  by  respective  moment  arms 
//  x_bow_vertical  (+),  x_st€rn_vert  (-) 

{  (AlT/_bov;_vertical  *  fabs  (AUV_bow_vertical )  *  x_bow_vertical ) 

+  ( AW_stern_vertical  *  fabs  {AUV_stern_vertical )  *  x_stern_vertical )  )  ; 

//  EPSILON  terms  have  been  removed  due  to  revised  equations  of  motion 
//  ^  rho2  *  L4  *  M_qn  *  U*Q  *  EPSILON 

/,^  +  rho2  ^  L3  *  {M_wn  *  U^W  +  M_dsn  *  U*fabs(U)  *  delta_planes_stern) 

//  *  EPSILON; 

//////////////////////////////////////////////////////////////////////////////// 

if  {TRACE)  //  Pitch  TRACE 
{ 

cout  pitch  terTrO  =  "  -  ( I_x  -  I_z)  ^  P*R  +  I_xy  *  Q*R  -  I_y2  *  P*Q 

-  I_xz  *  (P2  -  R2)  <<  endl ; 


cout  "term2="  -^<4  m  *  (  x_G  *  (  -U*Q  +  V*P)  -  z_ 
end!  ; 


(  -  V*R  +  W*Q) ) 


cout  «  "term3="  «  +  rho2  *  LS  *  (  M_pp  *  P2  +  M_pr  *  P*R  +  M_rr 

*  R*fabs  (R) 

+  M_q  *  Q 

+  M_qq  *  Q  *  fabs(Q))  //  hovering  pitch  drag 

«  endl ; 

cout  «  ‘'term4=*'  «  +  rho2  *  L4  *  (  M_uq  *  U*Q  +  M_vp  *  V*P  +  M_vr  *  V*R) 

«  endl; 

cout  «  '•term5=’'  «  +  rho2  *  L3  *  (  M_uw  *  U*W  +  M_vv  *  V2 

+  U*fabs(U)  *  (  M_uu_del ta_bow  *  del ta_p lane s_bow 

+  M_uu_delta_stern  *  delta_planes_stern)  ) 

«  endl ; 

cout  «  *'term6=’'  «  +  pitch_integral 
«  endl ; 

cout  «■  "term7="  «  -  (x_G  *  Weight  -  x_B  *  Buoyancy)  *  cosTHETA  *  cosPHI 


198 


cou 


CCl 


«  endl; 

<•<  *'termS=:'’  «  -  ( z_G  *  Weight 
end!; 


z_B  *  Buoyancy)  *  sinTHETA 


// 


"term9=‘'  «  +  {2.0  /  (24.0  *  24.0)) 

//  each  thruster  2.0  lb  per  24V  signal  squared 
//  multiplied  by  respective  moment  arms 
//  x_bow_vertical  (+) .  x„stern_vert  {-) 

*  {  (AUV_bow_vertical  *  fabs  {AUV_bow_vertical )  *  x_bow_vertical ) 

+  (AW_stern_vert  ical  ^  fabs  (AUV_stern_vertical )  *  x_stern_vertical )  ) 
«  endl ; 

jt  "  termlO  ,  terml  1=  “  «  "EPSILON  terms,  no  longer  used" 

«  endl; 

cout  •r'<  •' terml 0="  «  +  rho2  *  L4  *  M_qn  *  U*Q  *  EPSILON 
endl  ; 

cout  •r'<  "termll  =  "  +  rho2  *  L3  *  {M_wn  *  U*W  +  M_dsn  *  U*fabs(U) 

*  delta_planes_stern) 

*  EPSILON 

endi ; 


•'///// ////////////////////////////////////////////////////// ///////////// 


rhs  [YAW  ]  =  //  Yaw  Motion  Equation  right  hand  side 


-// 


-  (I^  -  I_X; 

»  P*Q  +  I_xy  * 

(P2  -  Q2) 

+  I_Y2  ^  P*R  -  I_xz  Q*R 

-  m  *  f  x_G  * 

(  U*R  -  W*P)  • 

-  y_G 

(  -  V*R  +  W*Q) ) 

^  rho2  *  L5  * 

{  N_pq  *  P*Q 

+  N_qr  * 

Q^R 

+  N_r  *  R 
+  N_rr  *  R  * 

fabs  (R)) 

// 

hovering  yaw  drag 

+  rhc2  *  L4 

{  N_up  -  U*P 

^  N_ur  *  U*R 

+  N_vq  *  V*Q 

N_wp  ^  W*F 

+  N_wr  W*R) 

+  rho2  ^  L3 

(  N__uv  U*V 

+  N_vw  *  V^W 

+  U*fabs (U)  * 

-  U*fabs(U)  * 

N_uu_del  ta_rb 
N_uu_de  1 1  a_r  s 

*  del ta_rudder_bow 

*  del ta_rudder_s tern) 

yaw_integral 
(x_G  *  Weight 
(Y— G  *  Weight 


x_B  *  Buoyancy)  *  cosTHETA  *  sinPHI 
y_B  *  Buoyancy)  ♦  sinTHETA 


(2.0  /  (24.0  *  24.0))  //  each  thruster  2.0  lb  per  24V  signal  squared 

//  multiplied  by  respective  moment  arms 
*  {  (AW_bow_lateral  *  fabs  (AUV_bow_lateral )  *  x_bow_lateral  ) 

+ (AUV_stern_lateral  *  fabs  {AUV_stern_lateral )  *  x_stern_lateral  )) 

rho2  *  L2  *  C_d0 

*  (  square  (speed_p)er_rpm)  *  0.5  //  propeller  yaw 

*  (  AUV_port_rpm  *  f  abs  (AUV_port_rpm)  ♦  y_port_p  rope  Her 
+  AUV_stbd_rpm  *  f  abs  (AUV_stbd_rpm)  *  y_stbd_p  rope  Her) 
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-  U  *  f abs (U) ) ; 

//////////////////////////////////////////////////////////////////////////////// 
if  iTR.hC£)  if  Yaw  TRACE 

ccut  yaw  terTTil="  -  [\_y  -  i_x)  *  P*C  +  I_xy  *  (P2  -  Q2) 

+  I-YZ  *  -  I_X2  *  Q*R 


:out;  ■  “terTn2="  <• 


m  *  {  x_G  *  (  U*R  -  W*P)  -  y_G 


-  V*R  +  W*Q} ) 


couc  "tenTi3=‘'  «  +  rho2  *  L5  *  {  N_pq  *  P*Q  +  N_qr  *  Q*R 

+  N_r  *  R 

H-  N_rr  R  tabs  (R)  )  //  hovering  yaw  drag 

-  "  endi ; 

ooun  ■■■'  "tenn4-"  --<  +  rho2  *  L4  *  (  N_up  *  U^P  +  N_ur  *  U*R  +  N_vq  *  V*Q 

+  N_wp  *  W*P  +  N_wr  *  W^R) 

<<  endl; 

cout  «  "terTn5=*'  «  +  rho2  *  L3  *  {  N__uv  *  U*V  +  N_vw  *  V*W 

U*fabs(U)  *  N_uu_delta_rb  *  del ta_rudder_bow 
-  U^fabs(U)  *  N_uu_delta_rs  *  del ta_rudder_stern) 

endi ; 

ccun  *•  ''i:erK,c="  <*:  -  yaw_incegral 

*r-.-  endi; 

rout  <:*'  "Lern-i7=‘’  4  (x^G  *  Weight  -  x_B  *  Buoyancy)  *  cosTHETA  *  sinPHI 

endi; 

cout  -:'■  "teriTi8=:"  «  +  {y_G  *  Weight  -  y_B  *  Buoyancy)  *  sinTHETA 

er.d^  ; 

cout  «  •‘tenri9='’  «  -  (2  . 0  /  (24  . 0  *  24 . 0)  ) 

//  each  thruster  2.0  lb  per  24V  signal  squared 
//  multiplied  by  respective  moment  arms 
*  '  ;A'LP7_bow_lateral  *  fabs  (AUV_bow_lateral )  *  x_bow_lateral  ) 

(AW_stern_lateral  *  fabs  (AUV_stern_lateral )  *  x_stern_lateral  )} 

endi; 

cout  •' termlOn "  «  -  rho2  *  L2  *  C  dO 


«  endi ; 


(  square  ( speed__per_rpm)  *  0.5  //  propeller  yaw 

^  (  AUV_pcrt_rpm  *  f  abs  (AUV_port_rpm)  *  y_port_p  rope  Her 
AUV_stbd_rpm  *  f  abs  (AUV_stbd_rpm)  *  y_stbd_p  rope  Her) 
-  U  ^  fabs(U) ) 


/////////z 


//  debug: 


if  (TRAC 
{ 


cout  <<  “  SURGE  =  “  <<  SURGE  «  endi ; 

cout  <-<r  SWAY  =  *•  «  SWAY  «  endi; 
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cout  -"<■ 

II 

HEAVE 

<< 

HEAVE 

« 

endl 

COUC  *"< 

II 

ROLL 

<< 

ROLL 

« 

endl 

cour 

II 

PITCH 

<< 

PITCH 

« 

endl 

coui: 

II 

YAW 

<< 

YAW 

« 

endl 

CCUl 

"rhs 

[SURGE] 

- 

<< 

rhs 

[SURGE] 

« 

endl 

cout  << 

"rhs 

[SWAY  ] 

= 

<< 

rhs 

[SWAY  ] 

« 

endl 

cout 

"rhs 

[HEAVE] 

<< 

rhs 

[HEAVE] 

« 

endl 

cout  << 

“rhs 

[ROLL  ] 

<< 

rhs 

[ROLL  ] 

« 

endl 

COUC 

“rhs 

[PITCH] 

= 

<< 

rhs 

[PITCH] 

« 

endl 

cout  <:< 

“  rhs 

1  YAW  ] 

= 

<< 

rhs 

[YAW  ] 

« 

endl 

//  cout  <<  “mass_inverse :  "  «  endl ;  print_inatrix6x6  (inass_inverse)  ; 


cout  <<•  "velocities: 

<"  «  U 
«  P 

V  V 

V  V 

>  o 

V  V 

V  V 

s  s 

*  s 

V  V 

V  V 

U  H 

! 

tl  M 

«  W  «  " 

«  R  «  ">"  «  endl 

COUC  "RHS: 

print_matrix6 

(rhs] 

I  ; 

liis 

[SURGE] 

o 

o 

II 

rhs 

[ SWAY  i 

=  .  U  ; 

r S 

i  H  E A  \‘  L  ; 

=  U  .  U  ; 

rns 

ihCLL  j 

=  0.0; 

r  s 

;  rC.  T  H  j 

=  .  'J  ; 

rns 

:  YAW  : 

=  Cj  .  l;  ; 

/  /  /  !!  f !:  i  I  !!  i  !  i  !  I  i  i  !  I !  f  !  II I  !  I  N  i  I U  f  !  I !  H  n  !  I  n  t  n  !  I !  1 1 II J II I N  U  H  H  i  !  n  n  !  I !  I  i 

:  :  calculate  new  accelerations  rriatrix  using  mass_inverse  u  rhs,  print - // 

n:ul trply bx6_6  (mass_inverse,  rhs,  new_acceleration )  ; 


cout  <<  "Accelerations:  " ;  print_inatrix6  (new_acceleration) ; 

1 


II  limi 

t  accelerations  -- 

— 

c  1  amp  ! 

new_accelerati on 

[SURGE] , 

-5.0, 

5.0, 

"new^acceleration 

[SURGE] 

")  ; 

clamp  i 

new^a  cceleration 

[ SWAY  ] , 

-5.0, 

5.0, 

" n e w_a  cceleration 

[SWAY  ] 

")  ; 

c  .L  an’ip  1  S' 

new_a  cceleration 

[HEAVE] , 

-5.0, 

5.0, 

" new_a  cceleration 

[HEAVE] 

")  ; 

c  1  amp  •;  h 

ne w_a  cceleration 

[ ROLL  ] , 

-5.0, 

5.0, 

"new^acceleration 

[ROLL  ] 

")  ; 

c  1  amit;  (  h 

new_accelerati on 

[PITCH] , 

-5.0, 

5.0, 

“new_acceleration 

[PITCH] 

")  ; 

c 1  amp  { & 

new_acceleration 

[ YAW  ] , 

-5.0, 

5.0, 

“new_acceleration 

[YAW  ] 

// 


//  find  velocities  by  integrating  averaged  accelerations - // 

//  (Heun  integration) 


new_veloci ty 

[SURGE] 

=  0.5 

* 

(u_dot 

+ 

ne w_a  cceleration 

[ SURGE 

] ) 

* 

dt 

4 

U; 

new_veloci ty 

(SWAY  ] 

=  0.5 

* 

(v_dot 

+ 

new_acceleration 

[SWAY 

3) 

* 

dt 

4 

V; 

new_veloci ty 

[HEAVE] 

=  0.5 

* 

( w_do  t 

■I- 

new_acceleration 

[HEAVE] ) 

■* 

dt 

4 

W; 

new_veloci ty 

[ROLL  ] 

=  0.5 

★ 

(P__dot 

4 

new__ac  ce  1  e  r  a  t  i  on 

[ROLL 

3) 

* 

dt 

4 

P; 

new^veloci ty 

[PITCH] 

=  0.5 

* 

(q_dot 

+ 

new_acceleration 

[PITCH] ) 

* 

dt 

4 

Q; 

new_veloci ty 

[YAW  ] 

=  0.5 

* 

(r_dot 

+ 

new_acceleration 

[YAW 

]  ) 

* 

dt 

4 

R; 

//  find  velocities  by  integrating  instantaneous  accelerations 
//  {this  method  is  less  accurate  and  is  not  used,  although  at  small 

//  timesteps  the  difference  is  negligible) 

//  (Euler  integration) 

//  new_velocity  [SURGE]  =  (new_acceleration  [SURGE])  *  dt  +  U; 

//  new__veloci ty  [SWAY  ]  =  {new__acceleration  [SWAY  ])  *  dt  +  V; 


201 


!  ‘  new_velocity 
//  new^velocicy 
''  e ^  o Cl  t y 

/  /  n^iw^veloci ty 


[HEAVE] 
[ROLL  ] 
[PITCH] 
[  YAW  ] 


(nGw_acceIeration  [HEAVE] ) 
{new_acceleraticn  [ROLL  ] ) 
{new_acc€lerat ion  [PITCH]) 
(new_acceleration  [YAW  ] ) 


dt  +  W; 
dt  +  P; 
dt  +  Q; 
dt  +  R; 


'T,  tnat  surge  velocity  may  be  negative  under  to  model  constraints 

//'  but  this  is  a  probieiri  so  it  is  clamped  to  be  non-negative 

ciamp  iSc  new_velocity  [SURGE],  -3.0,  3.0,  “new_velocity  [SURGE]  velocity"); 

//  3.0  is  set  arbitrarily  high 

/  '  update  iJUVBody  state  accelerations  to  newly-calculated  values  - // 

u_dot  =  new_acceleration  [SURGE]; 
v_dot  =  new_acceieration  [SWAY  ]; 
w_dot  =  new__acceier3tion  [HEAVE]; 
p_dot  =  new_acceleration  [ROLL  ] ; 

=  new_acceleration  [PITCH]; 
i__dot  =  new _ acce j. era ti on  [YAW  ]  ; 

:  '  calculate  world  coordinate  system  linear  &  angular  velocities  - // 

;/  see  Cocke  Figure  10  for  corrections  to  Healey  equations  for  x/y/z_dot: 

//  also  Healey  course  notes  eqn  (26)  and  Frank-McGhee  corrected  paper  (A. 8) 

x_dot  =  AUY^_oceancurrent_u 


U  ^  cos 

(PEI) 

* 

cos 

(THETA) 

- 

V  *  (cos 

(PSI) 

★ 

sin 

(THETA) 

*  sin 

(PHI) 

-  sin 

(PSI) 

*  cos (PHI) ) 

W  *  (cos 

(PSI) 

* 

sin 

(THETA) 

*  cos 

(PHI) 

+  sin 

(PSI) 

*  sin (PHI ) ) 

y  __o  c  t  = 

/A  u  V  o  c  e  a  Ti 

curren 

; 

_y 

- 

ij  **  si n 

(PSI) 

* 

CCS 

(THETA) 

V  \  s  i  n 

(PSI) 

★ 

sin 

(THETA) 

*  sin 

(PHI) 

+  cos 

(PSI) 

*  cos (PHI ) ) 

- 

W  -  (sin 

(PSI) 

* 

sin 

(THETA) 

*  cos 

(PHI) 

-  cos 

(PSI) 

*  sin(PHI) ) 

z_dot  = 

AUv'_ocean 

curren 

t. 

_w 

-  u  *  sin  (THETA.) 

+  V  *  cos  (THETA)  *  sin  (PHI) 

-  W  *  cos  (THETA)  *  cos  (PHI); 

phi_dot  =  p  +  Q  *  sin  (PHI)  *  tan  (THETA) 

+  R  *  cos  (PHI)  *  tan  (THETA); 

theta_dot  q  *  cos  (PHI) 

-  R  *  sin  (PHI) ; 


if  (cos  (THETA)  ==  0.0) 

{ 

cout  «  "UUVBody :  :  integrate_equations_of_inotion  ():  "  «  endl  ; 

cout  «  cos  (THETA)  ==  0.0  so  psi _ dot  set  equal  to  zero."  «  endl; 

psi_dot  =0.0; 

} 

else  psi^dot  =  (Q  *  sin  (PHI)  +  R  *  cos  (PHI))  /  cos  (THETA); 
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Vector3D  linear_rates  =  Vector3D  (x_dot,  y_dot,  2_dot); 

If  (TRACE; 
i 

:cu  t  •  • ;  end I ; 

cont  <<  ’■<x_dot,  y_dot,  z_dot>  =  ”  «  linear_rates  «  endl; 

ccut  «  “  magnitude  =  "  «  linear_rates .magnitude  () 

<•<  endl; 

) 

VectcrBD  euler_rates  =  Vector3D  (phi_dot,  theta_dot,  psi_dot) ; 
if  (TRACE) 

{ 

ccut  «  ''<phi_dot,  theta_dot,  psi_dot>  =  "  «  euler_rates  «  endl; 
cout  «  magnitude  =  “  «  euler_rates  .magnitude  () 

<-  endl ; 

//  calculate  world  coordinate  system  hoiTiogenous  transform  matrix  - // 

Hi’rutiix  Kincremental  =  Hmatrix  ();  //  default  initialization 
Hincremental . set_posture  (  P  *  dt,  Q  *  dt,  R  *  dt  ); 

KincreiTientai  .  rotate  (  PHI,  THETA,  PSI  ); 

double  w_x  =  Kincremental . phi_value  (); 
double  =  Hincremental  .  theta_value  (); 

douole  w_z  =  HincreiTiental  . psi_value  (}; 


Ve::cr3r  world_rates  =  Vector 3D  (w_x,  w_y,  w_2) ; 
if  -TRACE; 

=  **  «  world_rates  «  endl; 
magnitude  =  '*  «  world_rates  .magnitude  () 


rt-.atiiv  Hrevisedl  =  Hmatrix  u;  //  default  initialization 
Hrevisedi . incremental_rotation  (  phi_dot,  theta_dot,  psi_dot,  dt  ); 
Hrevisedl . incremental_translation  (  U,  V,  W,  dt  ); 

Hmatrix  Hproductl  =  Hprevious  *  Hrevisedl; 

Hmatrix  Krevised2  =  Hmatrix  (  x_dot  *  dt,  y_dot  *  dt,  2_dot  *  dt, 

phi_dot  *  dt,  theta_dot  dt,  psi_dot  *  dt )  ; 

Hprevious  =  Hproductl; 

//  translate  and  rotate  and  update  time  in  RigidBody  state  - // 

//  note  world  coordinate  system  is  used  by  RigidBody: 

set_angular_veloci ties  (phi_dot,  theta_dot,  psi_dot) ; 

set_linear_velocities  {  x_dot,  y_dot,  z_dot) ; 

set_time_of_posture  (current_uuv_time) ; 

update_Hma trix  (dt); 

if  (TRACE) 

{ 

cout  «  “incremental  hmatrix  =  “ ; 

Hincremental .print_hmatrix  {); 
cout  «  “revisedl  hmatrix  =  " ; 

Hrevisedl . print_hmatrix  (); 
cout  <<  "revisedD  hmatrix  =  "; 

HrevisedC  .print__hmatrix  (); 
cout  “productl  hmatrix  =  “ ; 
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Hproductl  .print_hinatrix  ( )  ; 

cout  <<  “original  hip.atrix  = 
hinatrix.print_h]Tiatrix  {)  ; 


cout  “substituting  productl  hmatrix'*  «  endl;- 
hrr.atrix  =  Hproductl; 

//  - 

/  /  Save  body-coordinate-sy stert  velocities  for  the  next  loop: 

U  =  new_veloci ty  [SURGE]; 

V  =  new_velocity  [SWAY  ] ; 

W  ^  new_velocity  [HEAVE]; 

F  =  new_veIocity  [ROLL  ] ; 

Q  =  new_veiocity  [PITCH]; 

R  -  new_veiocity  [YAW  ] ; 

//  cout  “world  U  =“  U  «  'V  x_dot 

/.'  cout  "world  V  =“  «■  V  «  'V  y_dot 

//  cout  «:  “world  W  «  W  «  ",  z_dot 
//  cout  <■<■  "world  P  =“  <<  F  <<  ",  phi_dot 

//  cout  “world  Q  =“  «  Q  «  'V  theta_dot 

//  cout  «  “world  R  ="  «  R  <<  ",  psi_dot 


=  "  <<  x_dot  «  endl; 

=  "  «  y_dot  «  endl; 

=  "  «  2_dot  «  endl; 

=  "  «  phi_dot  «  endl; 

=  "  «  theta_dot  «  endl ; 

=  "  «  psi_dot  «  endl; 


//  update  all  hydrodynarr.ics-inodei-provided  state  variables  in  AUV_globals .  h 
II  prior  to  retransmittal  to  AUV  via  AUVsocket 

AU\'_time  =  current_uuv_tiTne ;  //  mission  time 


AU^'O:  =  x_value  ();  // 
A'Jv_tu  =  y_value  ();  // 
A'T‘v_z  =  z_value  ();  // 
Airi_phi  =  phi_value  ();  // 
AL^7_theta  =  theta_value  ();  // 
A’JV_psi  =  psi_value  ();  // 

AUV_u  =  new_velocity  [SURGE];  // 
Airi_v  =  new^_veloci  ty  [SWAY  ];  // 
AU'v^'w  =  new_velocity  [HEAVE];  // 
Altv_p  =  new_velocity  [ROLL  ];  // 
AUV_g  =  ncrw^velocity  [PITCH];  // 
AlFv'^r  =  new_velocity  [YAW  ]  ;  // 

AUV_u_dot  =  u_dot;  // 
Ainv'_v_dot  =  v_dot;  // 
AUV_w„dot  =  w_dot;  // 
Airv'_p_dot  =  P„dot;  // 
AUV_q_dot  =  q_dot;  // 
AUV_r_dot  =  r_dot;  // 


X  position  in  world  coordinates 
y  position  in  world  coordinates 
z  position  in  world  coordinates 
roll  posture  in  world  coordinates 
pitch  posture  in  world  coordinates 
yaw  posture  in  world  coordinates 

surge  linear  velocity  along  x-axis 
sway  linear  velocity  along  y-axis 
heave  linear  velocity  along  x-axis 
roll  angular  velocity  about  x-axis 
pitch  angular  velocity  about  y-axis 
yaw  angular  velocity  about  z-axis 

linear  acceleration  along  x-axis 
linear  acceleration  along  y~axis 
linear  acceleration  along  x-axis 
angular  acceleration  about  x-axis 
angular  acceleration  about  y-axis 
angular  acceleration  about  z-axis 


AUV_x_dot 

=  x_dot; 

// 

AUV_y_dot 

=  y_dot; 

// 

AUV__z__dot 

=  z_dot; 

// 

AUV_phi„dot 

=  phi_dot; 

// 

AUV_theta_dot 

=  theta_dot; 

// 

AUV_psi_dot 

=  psi_dot; 

// 

Euler  velocity  along  North-axis 
Euler  velocity  along  East-axis 
Euler  velocity  along  Depth-axis 
Euler  rotation  rate  about  North-axis 
Euler  rotation  rate  about  East-axis 
Euler  rotation  rate  about  Depth-axis 


#undef  UDOT 
#undef  VDOT 
#undef  WDOT 
#undef  PDOT 
#undef  QDOT 
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F. 


AUVsockeLC  Communications  with  a  Networked  AUV 


^-■*-»*-»--*'*TTk-*-*»-r^X»*************Tr*****'*-***»-***********'»-*******»**»'-***'»r************* 

'program:  AUVsocket.C 

Description:  socket  from  auv  execution  level  to  Irix  auv  virtual  world 

receives  partial  telemetry,  returns  full  telemetry  with 
dynamics  parameters  added 

AUVsocket.C  is  a  function  library  used  by  UUVBody 

AU'v^socket  also  provides  voice  server  interface 

Revised:  28  October  94 

Compilation:  unix>  make  AUVsocket 

Original  bases:  os9server.c,  dynamics. c 

References:  (1)  (Gespac-provided )  EVIRA  EVLAN-11  Ethernet  Data  Link 

Controller  for  the  G64/G96  Bus/EVTCP  Internet  Package 
technical  manuals 

(2)  Internetworking  with  TCP/IP  Volume  I:  Principles, 

Protocols  and  Architectures,  Douglas  E.  Comer, 

Prentice  Hall,  Englewood  Cliffs  NJ,  1991 

i3)  Internetworking  with  TCP/IP  Volume  II:  Design, 

Implementation  and  Internals,  Douglas  E.  Comer  and 
David  L.  Stevens,  Prentice  Hall,  Englewood  Cliffs  NJ,  1991 

(4)  IRIX  Network  Programming  Guide,  Silicon  Graphics  Inc. 

(5)  An  Advanced  4.3BSD  Interprocess  Communication  Tutorial, 
Samuel  J.  Leffler,  Robert  S.  Fabry,  William  N.  Joy, 

Phil  Lapsley,  Steve  Miller  and  Chris  Torek,  undated 

{6;  Real-Time  Programming  Tutorial,  Bill  Mannel,  SGI  Expo, 
Silicon  Graphics  Inc.,  23  May  93 

(7)  "Say..."  Axel  Belinfante's  speech  server  at  University 
of  Twente,  Netherlands,  which  also  uses  Nick  Ing-Simmons^ 
phoneme  synthesizer  'rsynth': 
http: //utisi79 .cs .utwente.nl : 8001/say/? 

Status:  Tested  satisfactorily 


Future  work:  Consider  implementation  as  an  inetd  'superserver'  daemon  or 

just  closing  socket  when  done,  reopening  &  waiting 

Consider  bounds  diagnostic  checking  on  values  transferred 
over  socket 

Fix  type  mismatch  on  signal  ()  calls  -  very  gnarly  problem! 
Comments  and  suggestions  are  welcome! 


/************^**************************:fc*************************^t************/ 

#ifndef  AUVSOCKET_C 

#define  AUVSOCKET_C  /*  prevent  errors  if  multiple  #includes  present  */ 

/*****★**★*»*******★**★******★★**★*************•**★***★****★*★***★****♦*****♦*:**  ^ 

# define  _BSD_SIGNALS 
#include  <ctype,h> 

^include  <signal.h> 

#include  <stdio.h> 

^include  <sys/types . h> 
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# include  <sys /socket . h> 

#incl  ude-  <netinet  /in  .  h> 

#include  <netdb.h> 

# include  <string.h’> 

^include  <tirne.h> 

<*irj.':iudt  -'  Stdlib.  h:- 
I  ]*,  c  I  u  d  e  ‘-’U  n  i  s  t  d .  Yi  y 

#tnclude  "AWglobals ,  H"  //  global  state  vector 
^include  "Quaternion . C"  //  degrees /radians  conversions 


//  extern  "C"  entries  provide  function  compatibility  with  C++  compiler  -  -  -  -  - 
extern  "C"  { 

void  bzero  (void  *b,  int  length) ; 

void  bcopy  (const  void  *src,  void  *dst,  int  length); 
int  strncmp  (const  char  *sl,  const  char  *s2,  size_t  n) ; 

char  *strcpy  (char  *sl,  const  char  *s2); 
char  ^strncat  (char  ^sl,  const  char  *s2,  size_t  n); 

//  reference  p.  434  Kelley  k  Pohl  "A  Book  on  C", 

/ /  /usr/include/sys/signal .h  and  /usr /include/signal .h 

//  int  ( ^signal (int , int  (*}(int,  ...))) (int,  ...); 


.  ^  One  stream  socket  is  used  with  adequate  throughput  */ 

■'*  (although  two  could  work,  no  performance  improvement  is  expected)  */ 

/*  Be  careful  that  you  reserve  these  port  numbers  to  prevent  collisions  */ 

/*  from  other  processes  requesting  ports  on  your  system:  */ 

^rdtrfint^  :  ISBF;IDGE_TCP_PORT  2056  /*  disbridge  1.3  program,  server  k  client  */ 

^define  NP£h'ET_MCAST_PORT  3111  /*  Mike  Macedonians  multicast  DIS  2.0.3  ^/ 

/*  NP£  Autonomous  Underwater  Vehicle  (AUV)*/ 
/*  Underwater  Virtual  World  (UVW)  */ 

tdefine  AUVSIM1_TCP_PORT_0  3210  /*  osPsender  <==>  os9server  test  programs  */ 

idefine  AUVSIMl_TCP_PORT_l  3211  /*  auv  execution  level  <==>  virtual  world  */ 

#define  AUV£IMl_TCP_PORT_2  3212  /*  auv  execution  <==>  tactical  (networked)*/ 

#define  AUVSIMl_TCP_PORT_3  3213  /*  port  for  future  use  */ 

^define  AUVSIMi_TCP_PORT_4  3214  /*  port  for  future  use  */ 

#define  AUVSIMl_TCP_PORT_5  3215  /*  port  for  future  use  */ 

#define  AUVSIMl_TCP_PORT_6  3216  /*  port  for  future  use  */ 

#define  AUVSIMl„TCP_PORT_7  3217  /*  port  for  future  use  */ 

^define  AUVSIMl_TCP_PORT_8  3218  /*  port  for  future  use  */ 

i^detine  AUVSIM1_TCP„P0RT_9  3219  /*  port  for  future  use  */ 

#define  SOCKET_QUEUE_SIZE  5  /*  max  allowed  by  TCP/IP  */ 


#define  AUVSIM_EXECUTION_LEVEL_HOST_NAME  "auvsiml . or . nps . navy .mil " 
/*****★*★*****★★★***★★★*★★★**★★*★★**★**************★*****■***★**★**♦★★♦******★**/ 


/*  function  prototypes  *★*****♦**★**♦»*****************************★**♦****»***/ 


open_executi on_level_socket 
shutdown_sock€t 


(int  signal_thrown) ; 


int  read_f rom_execution_level_socket  ();  //  returns  -1  on  failure 
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void 

writ  e_to_execution_level_socket 

0  ; 

double 

norrrial  i  ze 

(double 

degs ) 

double 

nonrial  ize2 

(double 

degs) 

void 

AUV_global_ini tial ization 

0  ; 

/***<•♦*★**»•*■★****★*******★***■***■*»*** 
1*  global  variable  definitions  ****** 
static  int  socket  d 


k-*-t************x****^**********^t**’it******y^ 

r*****-************:*:*-***’****^*************/ 


Static  int  socket_descriptor , 

socket_accepted; 

socket^stream; 

static  int  socket_length  =  255;  /*  max  allowed  packet  size  */ 

static  int  by tes_received,  bytes_read,  by tes_written, 

bytes_left,  bytes_sent; 

static  cnar  comirtand_sent  [300], 

coiTimand_received  [300], 
command_copy  [300], 

remote_host_name  [60]; 

static  int  shutdowr:_signal_received  =  FALSE; 

static  FILE  *  checksoundf ile ; 

static  struct  sockaddr_in  server_address; 

static  struct  hostent  *  server_entity ; 

static  char  keyword  [81]; 

static  char  *  ptr_index; 

static  cnar  *  remote_buf fer ; 

static  int  state__variables_received; 

static  int  return_state_vector; 

static  char  www_execution_message_string  [300]; 

static  char  answer; 

static  int  socket_al ready ^opened  =  FALSE; 

static  int  audio_enabled  =  TRUE; 

y**»-x-»ry>-TCH****»**-*»**^****-)r*-*ir*-*****ir**x***:r**»********-*-****************»*****xy 

int  open„execut ion_level_socket  (}  /*  Initialize  communications  blocks  */ 

{ 

if  ( sockec_al ready ^opened  ==  TRUE) 

{ 

if  (TRACE) 

{ 

cout  «  ''open_execution_level_socket  ():  socket_already_opened,  " 

«  •*  returning"  «  endl; 

} 

return  (-1); 

1 

else  if  (TRACE)  cout  «  " open_execution_level_socket  ()  starting"  «  endl; 

/**********★*************★****★*******★******************★*****************♦*** ^ 
/*  Initialize  both  client  &  server  ********************************★***********/ 


/*  Signal  handlers  for  termination  to  override  net_open  ()  and  net_close  ()*/ 
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/*  signal  handlers.  Otherwise  you  are  unable  to  kill  this  program. 

^ it  defined ( sgi } 

/  /  signal  {SIGHUF,  shu tdown_socket } ;  /*  hangup  */ 

/ ..  sigiioi  shu tdown^socket )  ;  /*.  interrupt  character  */ 

y /'  signal  (SIGKILL,  shu tdown_socket )  ;  /*  kill  signal  from  Unix  */ 

//  signal  (SIGPIPE,  shutdown_socket ) ;  /*  broken  pipe  from  other  host  */ 

//  signal  (SIGTERM,  shu tdown_socket ) ;  /*  software  termination  */ 

^endi f 


/*  Initialize  server  ** »*******»***********************************ir***********/ 

/*  setup  to  listen  for  client  to  attempt  connection  */ 

( 

/*  Server  opens  server  port  *»*******************************»***********/ 

/*  Open  TCP  (Internet  stream)  in  socket  */ 

if  (  (socket_descriptor  =  socket  (AF_INET,  SOCK_STREAM,  0))  <  0  ) 

{ 

cout  <<  " open_execution_level_socket  ()  can't  'open'  stream  socket** 

<<  endl ; 
return  ( -  1 ) ; 

/ 

else  If  (TRACE) 

cout  " open_execu t i on_level_socket  ()  socket  'open'  successful** 

<<  endl ; 

/*  Bind  local  address  so  client  can  talk  to  server  */ 

#if  defined i sgi; 

bzero  ((void  *)  &server_address ,  sizeof  ( server_address ) ) ; 

#er]dl  f 

server_address . si n_f ami ly  =  AF_INET;  /*  Internet  protocol  family  */ 

/*  make  sure  port  is  in  network  byte  order  */ 

server_address . sin_addr . s_addr  =  htonl  (INADDR_ANY} ; 
server_address . sin_port  =  htons  {AUVSIM1_TCP_P0RT_1 ) ; 

if  (hind  ;  socket_descriptor , 

{struct  sockaddr  *)  &server_address, 

sizeof  (server_address ) )  <  0) 

{ 

cout  <<  "open_execution_level_socket  ()  socket  'bind'  unsuccessful" 

«  endl ; 
return  (-1); 

} 

else  if  (TRACE) 

{ 

cout  «  *'open_execution_level_sock€t  {)  socket  'bind'  successful" 
«  endl ; 

} 

/*  prepare  socket  queue  for  connection  requests  using  listen  */ 

listen  (socket_descriptor ,  SOCKET_QUEUE_SIZE) ; 
if  (TRACE) 

{ 

cout  «  "open_execution_level_socket  ()  socket  'listen'  complete  ..." 

«  endl; 

} 

/*  Server  'accept'  waits  for  client  connections  **********1^** *★******★★★*/ 

if  (TRACE) 

{ 


209 


couc  <■<  " operi_execution_level_socket  {)  socket  waiting  to  'accept'  . 
<■<  end  I ; 

} 

by tes_received  =  sizeof  (socket_descriptor) ; 

while  ( { socket_accepted  =  accept  (  socket_descriptor , 

&server_ad.dress , 

&bytes_received) )  <  1) 

if  (TRUE)  /*  blocking  code  */ 

{ 

cout  <<  "open_execut ion_level_socket  {)  socket  " 

«  "'accept'  unsuccessful,  sleeping  1  second  ..."  «  endl ; 
sleep  ( 1 } ; 

) 

cout  <<  " open_execution_level_socket  ()  connection  is  open  between  " 

<<  "networks."  «  endl; 

f  end  initiaiizatiori 

socket_strearri  =  socket_accepted ;  /*  server  */ 

socket_al ready _opened  =  TRUE; 

if  (TRACE; 

{ 

cout  «  "AU\^socket  SERVER:  socket_descriptor  =  "  «  socket_descriptor 
<<  endl ; 

cout  <r<  "  socket_accepted  =  "  «  socket_accepted 

'<  endl; 

cout  <<  "  socket  stream  =  "  «  socket  stream 


return  (TRUE) 


tr*********-****-*-*-****-*-*-****^ 
k************lt-»***-*-*'**'*-*-st*  . 


int  read_f roiTi_execution_level_socket  ()  //  using  global  variables  state  vector 


temporary  hold  variables  */ 

double  AUV_time_temp, 
AU'v^x^temp , 

AUV_p  h  i  _  t  emp , 

AUV_u_t  emp , 
AUV_p_temp, 

AU\'_x_d  o  c  _t  emp , 

AU'\/_ph  i  _do  t_t  emp , 


AUV_y_temp, 
AU\^_theta_temp , 
AUV_v_temp, 
AUV_q_temp, 

AU\^_y_do  t_t  emp , 
AUV_theta_dot_temp , 


AU"v/_del  ta_rudder_temp ,  AUV_delta_planes_temp, 


AUV_z_temp, 

AUV_p  s  i  _t  emp , 
AUV_w_temp, 
AUV_r_temp, 
AUV_2_dot_temp , 
AUV_p  s  i  _d  o  t  _t  emp , 


AUV'_port_rpm_temp , 
AUV_bow__vertical_temp , 
AUV_bow_l a  t  era l_t  emp , 


AUV_s  t  bd_rpm_t  emp , 

AUV_s t ern_ve r t i ca l_t emp , 
AUV_stern_lateral__temp, 


AUV_ST1 000_range_temp ,  AUV_ST72  5_range_temp , 

AUV_ST1 000_bear ing_temp ,  AUV_ST72  5_bearing_temp , 
AUV_ST1 0  0  0_s  t  reng  th_temp ,  AUV_ST72  5_s  t  rength„t  emp  ; 
start_index,  offset,  index; 


/****»***»*********************:k*************»**************^**^**^^^^^^^^**y 

/*  Receiver  block  */ 

/*********** *******^^***********************************tfc*******^***^^***^**^ 

for  (index  =  0;  index  <  300;  index++)  /*  uppercase  */ 

command_received  [index]  =  (char)  0; 


/*  listen  to  remote  host,  relay  to  local  network/program 
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=  socket_length; 
by!:es_received  =  0; 

pci_:r;dex  =  coininand_received  ; 

Mbyces_left  >  0)  &&  (by  tes_received  >=  0))  /*  read  loop  *********-^**  / 

bytes_read  =  read  { socket_stream,  ptr_index,  bytes_left); 

if  (byres_read  0)  by tes_received  =  bytes_read; 

else  if  (byces^read  >  0} 

{ 

bytes^left  -=  bytes_read; 

by  tes_received  +=  byces__read; 
ptr_index  +=  bytes_read; 

i 

if  (TRACE) 

{ 

cout  «  "  read_f roin_execution_level_socket  () 

cout  «  “receiver  block  loop  bytes_read  =  ''  «  bytes_read«  endl; 

/*  if  noching  is  waiting  to  be  read,  break  out  of  read  loop  */ 

if  ( (bytes_read  ==  0)  &&  (by tes_received  ==  0))  break; 


; by res_recei ved  <  05  /*  failure  */ 

if  (TRACE) 

cout  «  “ read_f rojr!_execut ion_level_socket  () 
cout  <<  "receiver  block  read  failed,  by tes_received  =  " 

«  bytes^received  <<  endl ; 

?tate_vector  =  FALSE; 

' rerurn_st ate_vecr or : ; 

else  if  (byr.es_received  ==  0)  /*  no  transfer  */ 

{ 

if  (TRACE) 

ccuc  “ read_f rorri_execut ion_level_socket  () 

ccut  "receiver  block  bytes_received  =  0  bytes"  «  endl  ; 

returri_state_vector  =  FALSE; 
return  { return_state_vector )  ; 


/*  (bytes_received  >  0)  =>  socket  read  OK,  print  result  if  valid  &  traced  */ 

if  (TRUE)  //  print  telemetry  record  &  key 

cout  «  command_received  «  endl; 

cout  «  "keyword,  t,x,y,  z, phi,  theta, psi,u,v,w,p, q,  r,  *• 

«  "x_dot  ,y_dot,  z_dot,phi_dot ,  theta_dot,psi_dot,  " 

«  "delta_rudder  ,delta_planes,  l_rpin,  r_rpm,  " 

«  "bow^vertical , bow_lateral , " 

«  "stern_vertical , stern_lateral , " 

«  "ST1000_bearing/range/strength, " 

«  "ST725_bearing/range/strength"  «  endl; 


state_variables_received  =  sscanf  (cominand_received, 

"%s  %lf  %lf  %lf  %lf  %lf  %lf  %lf  %lf  %lf  %if  %if  %if  %if  %if  %if  %if 
If  %lf  %lf  %lf  %lf  %lf  %lf  %lf  %lf  %lf  %lf  %lf  %lf  %lf  %if  %if  %if  \n-, 
k  ey wo  rd ,  & AUV_ t ime_t  emp , 

&:AUV_x_temp,  &AUV_y_teinp ,  icAUV_2_temp, 

6cAlT\’_p  h  i  _t  emp ,  &  AUV_t  h  e  t  a_  t  emp ,  &AUV_p  s  i_t  emp , 


211 


&A  emp ,  &  AU\^_v_t  emp ,  ScAUV_w_t  emp . 

5cAUv_p_temp ,  ScATJV_q_cemp ,  &AUV_r_t  emp , 

&AW_x_dot_temp,  &^AUV_/_dot_temp ,  ^tAUV_z_dot_temp, 

£cAlA/_phi_doc_t:emp,  £cAUV_theta_dot_temp,  ScAUV_j)si_dot_temp, 

^AlA'_de]  ta_rudder_cemp,  6cAUV_del  ta_planGS_temp, 

ScAlTV^  jport_rpm_temp ,  ScAW^s  t  bd_rpm_temp , 

^AUV_bow_vGrci cal_temp ,  &AUV__stern_vertical_temp, 

&cAir7_bow_l  a  t  er  a  l__t  emp ,  &cAUV_s  t  er  n_l  a  t  era  l_t  emp , 

&AOT_ST1 0  0  a_be a  r  i ng_t emp ,  ScAUV_STl  0  0 0_r ange_t  emp , 

ScAUV_ST1000_strength_temp, 

6.AW_ST7  :  5_bea  r i  ng_t  emp ,  ScAU\'_ST72  5_range_t:emp , 

^AlJV_ST725_strerigch_temp)  ; 

for  {index  =  0;  index  <  strlen  (keyword);  index++)  /*  uppercase  */ 
keyword  [index]  =  toupper  (keyword  [index]); 

:f  (TRACE)  printf  ( "  [ALVsccket  kGyword=:%s  ]  \n  ^  keyword); 

it  ; St ace_vari ables_received  ==  34)  //  transfer  was  OK  so  keep  new  values 

if  (TPw^.C'E;  pl’intf  { '■  [AOTsocket  st ate_variables_received  ==  34]  \n"); 


r  e  t  u  r  ri_s  cat  e_ve  c 
AW  time 


AUV_phi 

AUV_tket 

Airj_osL 

A'jv_u 

AUV  V 


AU\'_q 
AW'  r 


AU7_z_dot  = 

AW_phi_dot  = 

A 'Tv _ theta _ oct  = 

AW_P'£i_dot  = 

ATv_deI  ta_rudder  = 
AUV_dGlta_p  lanes  = 
A  W_p  o  r  t_rpm  = 

AITv_5tbd_rpm  = 

AUV_bow_vGrtical  = 
A  W_s  t  e  r  n_v  e  r  t  i  c  a  1  = 

A  u  V _ DOW _ 1  a  t  e  r  a 1  = 

ATv^^s  t  e  r  n_l  a  t  e  r  a  1  = 

AiW^STI  0  0  0_be  a  r  i  n  g  := 
AW_ST1 00  0_range 
AUV_ST1000_strength  = 
AUV_ST725_bearing 
AUV_ST725_range 
AUV_ST725_strength  = 


or  =  TPCuE; 

=  AlJV_time_tetnp; 

=  AUV_x_temp; 

=  AU\^_y_cemp; 

=  AUV_z_temp; 

=  radians  (AUV_jDhi__temp)  ; 

=  radians  (AUV_theta_tGmp ) ; 

=  radians  (AUV_psi_temp) ; 

=  AUV_u_temp; 

=  AUV_v_temp; 

=  AUV_w_temp; 

=  radians  {AUV_p_temp ) ; 

=  radians  (AUV_q_temp) ; 

=  radians  (AUV_r_temp) ; 

=  AUV__x_do  t_t  emp  ; 

=  A\JV_y_do  t_t  emp  ; 

=  AUV_z_do  t_t  emp  ; 

=  radians  (AU\^_phi_dot_temp )  ; 

=  radians  (AUV_theta_dot_temp)  ; 

=  radians  (AUV_psi_dot_temp )  ; 

=  radians  {AUV__delta_rudder_temp)  ; 

=  radians  (AU\^_delta_planGs_temp)  ; 

=  AUV_p  o  r  t  _r pm_ t  emp ; 

=  AUV_3  t  bd_r pm_t  emp ; 

=  AUV_bo  w_ve  rtical__t  emp ; 

i  =  AU\^_stern_vertical_temp; 

=  AUV_bow_lateral_tGmp; 

=  AU\^_stern_lateral_temp; 

1  =  AlJV_ST1000_bearing_temp; 

=  AUV_ST1000_range_temp; 

th  =  AUV_ST1000_strength_temp; 

=  AUV_ST725_bearing_temp; 

=  AUV_ST7  2 5_range_temp ; 

=  AUV_ST72  5_st  rength^temp; 


3e  /*  a  message  was  received  instead  of  telemetry 
if  (TRACE)  printf  ( " [AUVsocket  non- telemetry  received] \n"); 


return_state_vector  =  FALSE; 


if  (strcmp  {keyword,  "AUV_STATE*‘ )  ==  0) 

{ 

printf  ( 

“Garbled  telemetry  record  received  li!  variables_received=%d\n%s\n“ , 
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staCe_variables_received,  command_received) ; 
f flush  fstdout}; 

return_state_vector  =  FALSE; 

TRACE  =  TRUE; 

cin.get  (answer);  //  pause 
cour  '  endl ; 

else  if  (strcmp  (keyword,  “MAIL")  ==  0)  /*  Send  e-mail  request  */ 

( 

printi  {“^s^n",  command^received) ; 
system  f  command_received) ; 

I 

else  if  (strncmp  (command^received,  “KILL",  4)  ==  0) 

^  /*  KILL  prior  to  reloop*/ 

if  (TRACE)  printf  ( “ [AUVsocket  KILL]\n"); 

shutdown^socket  (0); 
return_state_vector  =  -3; 

) 

else  if  (strcmp  (keyword,  “TIME")  ==  0) 

{ 

if  (TRACE)  printf  ("[AUVsocket  TIME]\n"); 

sta te_variables_received  sscanf  (coniniand_received,  "%s%lf", 

keyword, 

&AUV_time_teiTip)  ; 

if  (state_variables_received  ==  2)  //  transfer  OK,  keep  new  values 

AUV_time  =  AUV_time_temp; 

return_state_vector  =  FALSE; 
printf  ("%s\n",  comimand_received)  ; 


printf  ("a  bad  time  command  was  received  & 
return_statG__vector  =  FALSE; 


ignored:  %s", 

command_received)  ; 


else  if  ((Strcmp  (keyword,  "POSITION")  ==  0)  M 
(strcmp  (keyword,  "LOCATION")  ==  0)  ) 

if  (TFAiCE)  printf  ( "  [ AUV^socket  POSITION]  \n" )  ; 

state_variables_received  =  sscanf  (command_received,  “ %s%lf %lf %lf “ , 

keyword,  &AUV_x_temp, 

& AUV__y_ t  emp ,  &AUV_z_t  emp )  ; 

if  (state_variables_received  ==  4 )  //  transfer  OK,  keep  new  values 

AUV_x  =  AUV_x_temp; 

AUV_y  =  AUV_y_temp; 

AUV_z  =  AUV_z_temp; 

return_state_vector  =  -4; 

printf  ("%s\n",  command_received) ; 

} 

else 

{ 

printf  ("a  bad  position  was  received  &  ignored:  %s“, 

command_received) ; 

return_state_vector  =  FALSE; 

} 

} 

else  if  ((strcmp  (keyword,  “ROTATION")  ==  0)  I  I 
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(9Lrcirip  (keyword,  "ORIENTATION*')  ==  0)) 

scate_variabies_received  =  sscanf  { command_received,  **%s%lf  %lf  %lf  " , 

keyword,  ficAUV_phi_temp, 

&AUV_theta_temp,  ScAUV_psi_temp)  ; 

if  { sta te_variables_received  ==  4)  //  transfer  OK,  keep  new  values 

{ 

AUV_phi  =  radians  (AUV_phi_temp) ; 

Air\/_theta  =  radians  (Ab^_theta_temp)  ; 

AU^^'_psi  =  radians  (AUV_psi_ternp )  ; 

return_state_vector  =  -4; 

print  f  ("^sXn",  coininand_recGived)  ; 

) 

else 

{ 

princf  ("a  bad  orientation  was  received  &  ignored:  %s", 

coiniTiand^received)  ; 

return_st ate_vector  =  FALSE; 

} 

) 

else  if  (strncinp  ( comma nd_received,  "AUDIBLE",  7)  ==  0}  /*  enable  sound  */ 

print  f  { "  [AU\^socket  AUDIBLE]  \n"  }  ; 

return^st ate_vector  =  FALSE; 
audic_en5bled  =  TRUE; 

rtrturi':  ( return_state_vector  •  ;  duplicate  command  copy  not  spoken  */ 

else  if  (strncmp  (command_received,  "SILENT",  6)  ==  0}  /*  disable  sound  */ 

printf  '"[AUVsocket  SILENT] \n"); 

return_state_vector  =  FALSE; 
audic_enabled  =  FALSE; 


if  ((strncmp  (keyword,  "AUPv^STATE " ,  10)  !=  0)  /*  non- telemetry  string  */ 

( audio_enabled  ==  TRUE)) 

/ 

^/**t[*-*r**-r**t********-k-»;t:**fr*****-r-*'K*-k‘kic-k***ic**-k-k-X-k-ktclc'k-**-k'k*ir-k*-k^-k****^ 

generate  audio  of  non- tel emet ry  line  passed  by  execution  level  */ 
Don  Brutzman  Naval  Postgraduate  School  brutzmanQnps .navy .mil  */ 

y  »yx*»Tt**»-»**-*-»Tt*****»-*»*»****»»»*****»»-»*****»»*»******************y 


start_index  =  0; 
offset  =  0; 

strcpy  ( comma nd_copy ,  " “ } ; 

/*  leading  blanks  in  query  are  stripped  */ 

for  (index  =  0;  index  <=  strlen  (command_received) ;  index  ++) 

{ 

if  (command_received  [index]  )  start_index  =  index  +  1; 

else  break; 

} 

/*  clean  up  any  extra  stuff  in  the  query  string  */ 

1 1  if  { comma nd_received  [strlen  ( comma nd_r ec e i ved ) ]  ==  '\n') 

If  command_received  [strlen  (command_received)  ]  = 

//  command_received  [strlen  (command_received) -1]  =  ' ; 

int  linelength  =  strlen  ( commander eceived) ; 
command_received  [linelength]  =  (char)  0; 

//  printf  ("  command_received  [linelength]  =  {%d}\n", 

//  command_received  [linelength]); 

for  (index  =  start_index;  index  <=  linelength;  index  ++) 
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} 

if 


if  ( (command^received  [index]  ==  '  M  && 

( comma nd_received  [index+l]  ==  '  '}) 
offset  =  offset  -  1;  //  ignore  multiple  blanks 
else  if  ( (command_received  [index]  ==  '  ')  && 

(isdigit  (command_received  [index+1]))} 

{ 

command_copy  [index  -  start_index  +  offset]  = 

//  don't  put  minus  before  number,  use  comma  instead 

} 

else  if  (command^received  [index]  ==  '  ' ) 

command__copy  [index  -  start_index  +  offset]  = 
else  if  (command_received  [index]  ==  '\n') 

command_copy  [index  -  start_index  +  offset]  =(char)  0; 
break ; 

} 

else  comiriand_copy  [index  -  start_index  +  offset]  = 

tolower  (command_received  [index]); 

printf  ("  Say...%s\n",  command_received) ; 

(TRACE)  printf  ( ” [Say . . . %s] \n " ,  command_copy) ; 


/*  build  the  remote  query  which  will  also  save  results  as  -object  */ 


St  rcpy  ( keyword,  *'  speech  /  " )  ; 
strcac  (keyword,  comrriand_copy)  ; 
strcat  (keyword,  ".au"); 

checksoundf ile  =  fopen  (keyword,  “r'*); 

if  (checksoundf ile  ==  NULL)  /*  file  not  previously  retrieved-do  so  */ 


printf  ("[%s  not  found,  making  remote  query  to  \n“,  keyword); 
printf  ("  the  Say.,  audio  server  in  Netherl ands ] \n" ) ; 

/ 

sprint f  {ww*w_execut ion_message_string , 

“vy-w  -o  speech/%s.au  http  : //www__tios.  cs  .utwente.nl/say/?%s"  , 

command_copy ,  command_copy ) ; 

if  (TRACE)  printf  ('‘[%s]\n'*,  www_execution_message_string)  ; 
system  ( www_execution_message_string) ; 
i 

el  se 

{ 

if  (TRACE) 

printf  {"[%s  was  found  locally,  no  remote  query  required]  \n*' , 
keyword) ; 

fclose  (checksoundf ile) ; 

} 

/*  build  string  to  play  the  audio  file,  then  do  so.  */ 

/*  don't  put  sfplay  in  background  or  audio  port  may  be  unavailable  */ 
/*  when  the  next  audio  message  is  sent  */ 

sprint f  (www_execution_message_string,  "sfplay  speech/%s . au* , 

command_copy ) ; 

if  (TRACE)  printf  {‘*{%s]\n“,  www_execution_message_string)  ; 
system  (www_execution_message_string) ; 

/*★****★»***★*★*****★**★*****★**★***♦****★*★★****★***★**★**♦*****♦**/ 
/*  audio  send  complete  *********************************************/ 
/****»*****★*»*★★*★★**♦*★***★★★*★*♦***★★*****♦*★★*★★♦*★★»★*★**♦*♦★*★/ 

} 

)  /*  end  processing  a  message  */ 
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/ 


rezu rn  (  re t u  iTi_s tat e_vec tor)  ; 

/ '^  end  read_f  roiTi_execution_level_socket  ();  * 


/**yr-*r-r**-k**±*-**ic****ic-kir***-k-k:k*****icic**ic**^*-k*-k-ki:-k-k***-kie-k**-k***-k-k*ic***ic-k***-kic-iii:t!jl 
.  »**x**x*****-*-»:**-«r******************x*******»**************»^*****************^^ 

void  wri te_to_execution_level_socket  {)  //  using  global  variables  state  vector 

/ 

sprint f  ( comma nd_s en t , 

"  uvv.^_state  %5.3f  %5.3f  %5.3f  %5.3f  %5.3f  %5.3f  %5.3f  %5.3f  %5.3f  %5.3f  %5.3f 

%5.3f  %5.3f  %5.3f  %5.3f  %5.3f  %5.3f  %5.3f  %5.3f  %5.3f  %5.3f  %5-3f  %5.3f  %5.3f 

%5.3f  %S.3f  %S.3f  %5.3f  %5.3f  %5.3f  %d  %5.3f  %5.3f  \n", 

AUV^_time,  AUV_x,  AUV_y,  AUV_2, 
ncrmalize2  (degrees  (AUV^phi)), 
normalize!  (degrees  (AU\^_theta)  )  , 
normalize  (degrees  (AW_psi)), 

Al7v_u  ,  Airv'_v ,  AUrv_w , 
normalize!  (degrees  (AUV_p) ) , 
normalize!  (degrees  (AU^/_g)  )  , 
normalize!  (degrees  (AUV_rj } , 

AU\'_x_doc,  AW^_dot,  AOT_2_dot, 
normalize!  (degrees  ( AW_phi_dot )  )  , 
normalize!  (degrees  (AOT_theta_dot ) ) , 
normalize!  (degrees  (AU\'_psi_dct )  ,)  , 
normalize!  (degrees  (AU\^_delta_rudder)  )  , 
normialize!  (degrees  { AU\'_del ta_pi anes  )  )  , 

Al^v_ii  c  r  t_  rpiT: ,  A'J'i^s  t  bd_r  pm , 

Airv'_bov;_vertical ,  AUV_stern_vertical , 

ALA- _bcw_i  a  t  e  r  a  1 ,  AU\'_s  t  er  n_l  a  t  e  r  a  1 , 

A/u/^crp;:  AUV_STiOOC_range,  AW_ST100  0_strength , 

AL^^_ST7  2  5_bea  ring,  Altv.ST?  2  5_range .  AW_ST7  2  5_s  t  reng  t  h 


if  (TRACE'  cout  ccmmand_sent  <<  endl ; 

/*^*'>^-r**tir******-^^i,irTi:***ictyricic******i:****-k*iricic-kici:***ic**-k***icic***ic**-kic*irir±icic*/ 

/*  Sender  block  */ 

/****-ir^ic**-k**ir****ir*ic**ir**->c*-^*-ki,iri,ir*****ir****-ki,ic*-****i,i,i,i,ir*-k***ic*ic****-^ic*:^^*/ 

by tes_received  =  strlen  ( comma nd_sent } ; 

if  ( Dy tes_received  <  Oj  /*  copy  failure  */ 

cout  <<  "wri  te_to_execution_level_socket  ()  ** ; 

cout  <<  "copy  from  command_received  unsuccessful"  «  endl; 

snut  dowries  ocket  (0); 

retu  rn ; 

} 


else  if  (by tes_received  >  socket_lengthj 
{ 

cout  «  “wri te_to_execution_level_socket  ()  " 

<<  “  send_t  el  erne  tr^'_t  observer  error:  "  «  endl; 
cout  «  "bytes_received  too  big  for  packet  socket_length " 

«  "  [bytes^received^"  «  by tes_received 

«  "]  >  [socket_length= “  «  socket_length  «  "];  " 

«  "string  truncated"  «  endl ; 

1 

bytes_left  =  socket_length; 

by tes_wri tten  =  0; 

ptr_index  =  comma nd_sent ; 

while  ( {bytes_left  >  0)  &&  (by tes_written  >=  0)}  /*  write  loop  **♦★★★*★*★★/ 

bytes_sent  =  write  (socket_stream,  ptr_index,  bytes_left); 
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if  (bytes_sent  <  0}  bytes_writ ten  =  bytes_sent; 

else  if  (bytes_senc  >  0) 

bytes_left  bytes_sent; 

bytes_wri tten  bytes_sent; 

ptr_index  +=  bytes_sent; 

} 

if  (TRACE  &&  (bvtes_written  !=  socket__length ) ) 

{ 

couc  <*•"  "wri te_to_execution_level_socket  {)  loop  bytes  sent 
<<  bytes_sent  «  endl ; 

} 

} 

if  (by tes_wri tten  <  0) 

i 

cout  "wri te_to_execution_level_socket  ()  send  failed,  ** 

<<  by tes_wri tten  «  "  bytes_written''  «  endl; 

I 

else  If  (TRACE) 

( 

cout  <''  " write__to_execution_level_socket  (}  total  bytes  sent 
«  bytes_wri tten  «  endl ; 


return ; 


/*  end  wri tG_t o_execution_level_socket  (); 


int  3hutdov;n_sockGt  {int  signal_thrown)  /*  Shutdown  block  */ 


int  clcse_result ; 


"shutdcwn_socket  ( "  «•  signal_thrown 
shutdown  in  progress  ..."  «  endl; 


snu tdcwn_signal_recelved  =  TRUE; 

/*  Nc  need  to  send  a  message  to  other  side  Chat  bridge  is  going  down,  */ 

since  SIGPIPE  signal  trigger  may  cause  shutdown  on  other  side  */ 

ciose_result  =  close  ( socket_stream) ; 


if  ( close_result  ==  -1) 

cout  <<'  "shutdown_socket  ()  close  {socket_stream)  failed"  «  endl; 
if  (TRUE)  cout  «  '‘shutdown_socket  ()  complete"  «  endl; 
return  (close^result ) ; 


}  /*  end  shutdown_socket  ()  */ 

y**»*****************^*******st************iir**********ie**'***********************^ 
^★•******  +  ***it******************************************************************y' 


double  normalize  {double  degs)  /*  degrees  input*/ 

{ 

double  result  =  degs; 


while  (result  <  0.0)  result 

while  (result  >=  360.0)  result 


360.0; 

360.0; 
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resul c ; 


^-'★*»**x:^x****»**-*-***^******************^*****it*********-i:***********************  j 
^/*»»**'»-*-»:***X***»*****»»*******T»r****x*******'*-***»*******-r*-T«r*****1>:****»i:*********  J 

double  normal ize2  (double  degs)  /*  degrees  input*/ 

{ 

double  result  =  degs; 

while  (result  <=  -180.0}  result  +=  360.0; 
while  (result  >  180.0)  result  -=  360.0; 

return  result; 

/■r*»->-**-#-***x*'*»-**:r**Tr*-<r*****-*-»-**»********-*-****-***********.*****:if*******'*'#*******  j 
.■•■>::f»-«:Tr»»-r*»»»»****-*-'Jt***»*******»*-#-*****'***********»****H:*************»********^fy 

void  AUV_global_lni tialization  () 

//  Variable  initialization  performed  separately  to  avoid  compiler  warnings  -  - 


AlTvh 

Wime 

= 

0.0; 

// 

mission  time 

AUV 

_del ta_rudder 

z: 

0.0; 

// 

positive  is  bow  rudder  _ 

AW 

_dGl ta_planes 

= 

0.0; 

// 

positive  is  bow  planes  _ 

AW 

_ijcrt_rpm 

= 

0.0; 

// 

propeller  revolutions  per 

minute 

AW 

_.3tbd_rpm 

= 

0.0; 

// 

propeller  revolutions  per 

minute 

AW 

_bow_vertical 

= 

0.0; 

it 

thruster  volts  24Y  =  3820 

rpm  no 

load 

AW' 

_3  ten  i _ V  e  r  t  i  c  a  1 

= 

0.0; 

// 

thruster  volts  24V  =  3620 

rpm  no 

load 

AW.^ 

_bow_l  at  era  1 

= 

0.0; 

// 

thruster  volts  24V  =  3820 

rpm  no 

load 

AW^ 

_stern_lateral 

= 

0.0; 

// 

thruster  volts  24V  =  3820 

rpm  no 

load 

AW 

^^depthwt*!  1 

0.0; 

// 

pressure  sensor,  units  are 

A’W/ 

^heading 

0.0; 

// 

gyrocompass  in  degrees 

AW^ 

_rol l_angle 

= 

0.0; 

// 

matches  intertial  sensor 

onboard 

KXN 

AW' 

j:-it  entangle 

0.0; 

// 

matches  intertial  sensor 

onboard 

AW 

AW7 

wol  l__rate 

= 

0.0; 

// 

matches  intertial  sensor 

onboard 

AW 

AW^ 

^i  tch_rste 

= 

0.0; 

// 

matches  intertial  sensor 

onboard 

AW 

AW^aw_rate 

= 

0.0; 

// 

matches  intertial  sensor 

onboard 

AW 

AW 

_hour 

= 

0; 

// 

internal  AW  OS-9  system 

time,  unused 

AWh 

^minute 

= 

0; 

AWh 

„sec  end 

= 

0; 
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AU\^_u_dot 

r: 

0.0; 

// 

linear  acceleration  along  x-axis 

AW_v_dot 

= 

0.0; 

// 

linear  acceleration  along  y-axis 

AU\'_w_dot 

= 

0.0; 

// 

linear  acceleration  along  x-axis 

AU\'_jj_dot 

s: 

0.0; 

// 

angular  acceleration  about  x-axis 

AUV_q_dot 

= 

0.0; 

// 

angular  acceleration  about  y-axis 

AUV__r_dot 

= 

0.0; 

// 

angular  acceleration  about  z-axis 

AUV_oceancurrent_ 

_u 

0.0; 

// 

Ocean  current  rate  along  North-axis 

AOT_o  ceancurrent. 

_v 

= 

0.0; 

// 

Ocean  current  rate  along  East-axis 

AUV_o  ce  a  n  c  u  r  r  en  t. 

_w 

= 

0 . 0 ; 

// 

Ocean  current  rate  along  Depth-axis 

//  Prior  time  loop  saved  variables 


AUV_t i me_p  r i o r 

o 

o 

II 

// 

mission  time 

AUV_x_pri or 

=  0.0; 

// 

X  position 

in 

world 

coordinates 

AU\'_y_pri  or 

=  0.0; 

// 

y  position 

in 

world 

coordinates 

A'J\-_2_l3rior 

=  0.0; 

// 

2  position 

in 

world 

coordinates 

AUV_p'hi_p'rior 

=  0.0; 

// 

roll  posture 

in 

world 

coordinates 

A  W_t  he  t  a_p  r  i  or 

=  0.0; 

// 

pitch  posture 

in 

world 

coordinates 

AW_psi_prior 

=  0.0; 

// 

yaw  posture 

in 

world 

coordinates 

//  Sonar-  &  terrain-model -provided  state  variables 


AlTv  _S  T 1 0  0  0_be  a  r  i  n  g  = 
AW_ST1 000__range 
AUV_S  T1000_strength= 


AW_ST7  2  5_bea  r  i  ng  = 
AiTv_ST7  2  5_range 
AW„ST7  2  5_strength  = 


0.0; 

// 

ST 

1000 

conical 

0.0; 

// 

ST 

1000 

conical 

0.0; 

// 

ST 

1000 

conical 

0.0; 

// 

ST 

725 

1 

X 

24 

0 . 0  ; 

// 

ST^ 

725 

1 

X 

24 

0.0; 

// 

ST 

725 

1 

X 

24 

pencil  beam  bearing 
pencil  beam  range 
pencil  beam  strength 


sector  beam  bearing 
sector  beam  range 
sector  beam  strength 


d:  f 


Al'VSQCKET_C  * ! 
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G. 


DISNetworkedRigidBody.C  DIS  Network  Connections  for  a  Rigid  Body 


:•! i i i I: i !! I ! i i ! i i ! I ! I II I  III  I ! I ! I ! I ! I ! I ! I ! I ! I ! I ! I ! I ! I ! I ! I ! I ! I ! I ! I ! I ! I ! I ! I ! I ! I ! I ! 

Frcgram:  DISNetworkedRigidBody.C 

Description:  DIS  network  interface  for  RigidBody  model 

Author:  Don  Brutzman 

Revised:  28  October  94 

System:  Irix  5,2 

omp  1  i  e  r  :  AN^:.  i  C  -t-  + 

Compilation:  irix>  make  dynamics 

irix>  CC  DISNetworkedRigidBody.C  -Im  -c  -g  +w 

Original  bases:  os9server.c,  dynamics. c,  DIS  library  test_it,c 

References:  (1)  IEEE  Protocols  for  Distributed  Interactive  Simulation  (DIS) 

Applications  version  2.0,  Institute  for  Simulation  and 
Training,  Universit  of  Central  Florida,  Orlando  Florida, 

28  May  1993. 

(2)  Macedonia,  Michael,  Zeswitz,  Steven,  and  Locke,  John, 
Distributed  Interactive  Simulation  (DIS)  multicast 
version  2.0.5,  Naval  Postgraduate  School,  February  94. 

'3)  Zeswitz,  Steven,  "NPSNET:  Integration  of  Distributed 

Interactive  Simulation  (DIS)  Protocol  for  Communication 
Architecture  and  Information  Interchange."  master's  thesis. 
Naval  Postgraduate  School,  Monterey  California,  28  May  1993. 

Units:  Virtual  world:  ft  ft/sec  radians  radians/sec 

DIS  PDUs:  m  m/sec  degrees  degrees/sec 

Future  work: 


Advisors:  Dr.  Mike  Zyda,  Dr.  Bob  McGhee  and  Dr.  Tony  Healey 


f  i  !  i  i  i i ! I  U  a  I !  U ! i ! f ! I H  a  I n ! i ! I ! I II I ! I ! I II n ( II i ! I i ! I ! U I i I ! I m ! I ! I ! I ! I N I  i ! 
^itndef  DISNETWORKEDRIGIDBODY.C 

#define  DISNETWORKEDRIGIDBODY.C  //  prevent  errors  if  multiple  #includes  present 

#define  METERS.PER.FT  0.3048 
tdefine  FT.PER.METERS  3.2808 

^include  ‘'RigidBod>^ . C *’ 

#include  "AUVsocket . C” 

#include  <string.h> 

^include  <bstring.h> 

#include  <time.h> 

//  DIS  includes.  See  Makefile  for  other  DIS  #include  files;  they  must  match. 

# include  " . . /DIS .mcast /h/disdef s . h" 

# include  " . . /DIS .mcast /h/ appearance .h" 


//////////////////////////////////////////////////////////////////////////////// 
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//  DIS  library'  function  prototypes  do  not  include  parameter  prototypes,  thus 
/.  needed  ones  are  included  here.  Function  definitions  are  located  in 

/n /el s ie /wor k3 /macedoni /net /mcast/ network/ src 

/  '  A-  ////////////////////////////////////////////////////////////////////////// 

class  DISNetworkedRigidBody  :  public  RigidBody 

private : 

' !  member  data  fields 

double  time_of_last_PDU; 

int  DIS_port_open; 

E  n  1 1 1  y I D  UUV_D I S_ i d ; 

E n  t :  t  y  Ty  pe  UUV'_D I  S_ ty  pe ; 

Enti tyStatePDU  *  UUV_DIS_pdu; 

clock_t  current_clock,  //  clock  tick  counter  for  current  loop 

initialclock, 
previous_clock, 
delta_clock; 

double  delta_time; 

int  PDU_skip_interval ; 

//  Multicast  Defaults  from 

/ /  / n ' el  si e/ work 3 /macedoni /net /mcast /network/utils /planes /pi anes . cc 

//  pointer  to  MULTICAST_PORT 
//  MULTI CAST_GROUP 
//  time-to-live 

pUDliC : 

■  member  const  rue cor  and  destructor  functions 
DISNetworkedRigidBody  ( } ; 

-DISNetworkedRigidBody  ()  {  /*  null  body  */  ) 

//  operators 

friend  ostream^  operator  « 

//  inspection  methods 


void 

print_networkedrigidbody 

0 

const; 

double 

t  ime_o  f  __1  a  s  t_PDU_va  1  u  e 

0 

const; 

int 

D I  S_p  o  r  t_op  en_va  1  u  e 

0 

const; 

int 

PDU_s  k  i  p_in  t  e  rva  l_va  1  ue 

0 

const ; 

//  modifying  methods 

void  set_PDU_skip_interval  (const  int  new_value) ; 

int  DIS_net_open  { )  ; 


(ostreamSc  out, 

DISNetworkedRigidBodySc  nrb}  ; 


cnar  MULTI CAST^PORT  [6]; 
cnar  MULTI CAST_GROUF  [20] ; 
cnar  port  [6]; 
cnar  group  [20]; 
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int 

DIS_net_wri te 

void 

DIS_net_close 

void 

DISNetworkedRigidBody, 

void 

set_time_of_last_PDU 

void 

set_ttl 

void 

set  group 

void 

set_por t 

0  ; 

0  ; 

lize  ( ) ; 

(const  double  new_time_of_last_PDU) ; 
(int  new_ttl); 

(char  *  new  group) ; 

(char  *  new_port); 


)  ; 

/  .  /  /  //  /  /  /V// //////////////////////////////////////////////////////////////////// 


- // 

/■/  constructor  inethods 

/  / - // 


DISNetworkedRigidEody : :  DISNetworkedRigidBod>'  ()  //  default  constructor 

?'C'V_sk:p_iritervai  =  i;  //  1  tenth  (s)  of  a  second  between  PDUs 

RigidE;ody_ini tiali ZG  ();  //  inherited  method 

t  ::!ie_of_last_PDU  =  0.0; 

DI S_port_open  =  FALSE; 

//  Initialize  clock  variables 
ini : : alclock  =  clock  i); 

previ ou3_cl ock  =  clock  (); 

//  Multicast  Defaults  from 

/ /  /n/elsie/work3/macedoni /net /mcast /network/uti Is/planes/planes , cc 

bzero  ( MULTI CAST^PORT,  6); 
bzero  (MULTICAST_GROUP,  20); 

strcoy  (MULTICAST^PORT,  "3111");  '  //  3111  npsnet  'standard'  address 

strcpy  (MULTICAST_GROUP,  "224.2.121.93"}; 


bzero  (port,  6) ; 
bzero  (oroup,  2  0)  ; 
strcpy  (port,  MULTI CAST_PORT) ; 
strcpy  (group,  MULTICAST_GROUP) ; 

tti  =  16;  //  multicast  ttl=16  stays  inside  NFS 

} 

)  I - // 

/ /  operators 

// - // 


ostreamS'  operator  «  (ostream&  out,  DISNetworkedRigidBodySc  nrb) 

{ 

nrb.print_rigidbody  (); 

out  «  "  time_of_last_PDU  =  "  «  nrb .  time_of_last_PDU  «  endl; 

return  (out)  ; 


// - // 

//  inspection  methods 

// - // 


void  DISNetworkedRigidBody : ;  print_networkedrigidbody  () 
const 
{ 
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print_rigidbody  {); 
ccur  <'<  "  time__of_last__PDU 


time  of  last  PDU  «  endl ; 


double  DISNetworkedRigidBody : :  time_of_last__PDU_value  {) 


return  trir!e_of_last_PDU; 


int  DISNetworkedRigidBod>^ :  :  DIS_port_open_value  () 
const 

reru rn  DIS_]:)ort_open ; 


inc  DISNetworkedRigidBody::  PDU_skip_interval_value  (} 


re t u  r n  PDU_s  k 1 p_i n  t e rva 1 ; 


/  modifying  methods 


void  DISNetworkedRigidBody::  set_PDU_skip_interval  (const  int  new_value) 

{ 

if  ( pru_skip_interval  >=  1)  PDU_skip_interval  =  new_value; 
el  se 
{ 

PE;-U_s  k  i  p_i  n  t  e  rva  1  =  10; 

cour  " PDU_skip_intervai  must  be  >=  1,  and  has  been  set  to  I."  «  endl; 


return ; 


■  riSb'etworkedRigidBody :  :  DIS_net_open  ()  //  Ref:  macedonia  include  files 

int  exercise_id  =  -1; 

int  coordlnate_system  =0;  //  0  =  flat  world,  1  =  round  world 

char  *  utm  file  =  " " ; 


irr  result  =  FALSE; 

if  (DI£_jrort_open  ==  FALSE) 

result  =  net_open  (port,  group,  ttl, 

exercise_id,  coordinate_system,  utm_file) ; 

//  result  =  net_open  (port,  group,  ttl);  //  old  version 

if  (result  ==  FALSE) 

{ 

DIS_port_open  =  FALSE; 

cout  «  "DIS_net_open_select  ()  failed"  «  endl; 


DIS_port_open  =  TRUE; 

//  atexit  (net_close) ;  //  ensure  port  is  reclosed  on  exit. 
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else 

{ 

cout  <<  "DIS_pcrt_open  ==  TRUE  so  DIS_net_opGn  {)  not  re-attempted. 

<<  endl ; 
i-rrsult  =  TRUE; 

return  result; 


int  DISNetworkedRigidBod^' :  :  DIS_net__wri  te  () 
current_clock  =  clock  {); 

delta_clock  =  currGnt_clock  -  previous_clock; 

dGlta_time  =  delta^clock  /  CLOCKS_PER_SEC; 

if  (TRACE)  cout  «  “DISNetworkedRigidBody : :DIS_net_writG :AUV_time  =  " 

«  AUV_time; 

//  do  not  write  faster  than  one  DIS  PDU  per  PDU_skip_interval  tenth-seconds 
//■  of  AUA'_time 

i  'int;  (Atnv'_cime  *  10,0}  %  PDU_skip_in nerval  0) 

I 

rf  (TFJiCE}  cout  <<  ",  returning."  <<  endl  ; 
return  (TRUE) ; 

/ 

else  if  (TRACE)  cout  <■<'  ",  sending  DIS  PDU."  «  endl  ; 
previous_clock  =  current^clock ; 

//  see  test^ic.c  ('client'  program)  in  DIS  library  for  examples: 

/ 1  / n/el sr e/wcrk3 /macedoni /net /mcast /network/src/test_i t . c 

UUV_DIS_id. address. site  =  SITE_IL_NPS;  //  DIS  standard  para  5. 3. 8. 1.1 

UU'v_DIS_id .  address  .host  =  (unsigned  short)  1; 

UUV_DI S_i d . enti ty  =  (unsigned  short)  1; 

//  force  the  #define  values  to  the  right  types 

UU'^’>-.UIS_type -entity^kind  =  (unsigned  char)  EntityKind^Platform; 

Uirv'_DIS_type . domain  =  (unsigned  char)  Domain_Subsurface; 

UUV_DIS_type . country  =  (unsigned  short)  USA; 

//  not  yet  defined  in  DIS  header  file 

//  UTTv^^DIS^type. category  =  (unsigned  char)  Category_ResearchMiscSub; 
UUV_DIS_type .  subcategory  =  (unsigned  char)  0; 

UUV_DIS_type . speci f ic  =  (unsigned  char)  0; 

Uir7_DIS_pdu  =  maliocEnti  tyStatePDU  ( )  ; 

//  fill  in  the  parameters  of  an  entity  state  PDU  (most  are  listed  in  pdu.h) 
this  assumes  there  are  no  articulated  parameters  (add  later)  «« 

//  Note  all  units  converted  to  SI  when  building  PDU 

//  DIS  ID  and  Type 
UU\^_DIS_pdu->enti  ty_id 
UCT'/^D  I  S_pdu  -  >en  t  i  ty_ty  pe 
UUV_DI  S_pdu  -  >f  orce_i  d 
UUV_DI S_pdu - >a 1 t_ent i ty_ty pe .extra 

//  Posture 

UUV_DIS_pdu->entity_location .X  =  AUV_x  *  METERS_PER_FT; 

UUV_DIS_pdu->entity_location.y  =  AUV_y  *  METERS_PER_FT; 

UUV_DIS_pdu->entity__location  .  z  =  AUV_z  *  METERS__PER_FT; 

UUV_DIS_pdu->entity_orientation.psi  =  (int)  degrees  (AUV_jsi )  %  360; 

UUV_DIS_pdu->entity_orientation, theta  =  (int)  degrees  (AUV_theta)  %  360; 

UUV_DIS_pdu->entity_orientation,phi  =  (int)  degrees  (AUV_phi )  %  360; 


=  UUV_DIS_id; 

=  UUV_DIS_type; 

=  ForceID_Whi te; 

=  (Extra)  0x00;  //  unused  extra  parameter 
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//  Linear  and  angular  velocities  in  body  coordinates/meters  by  DIS  standard 

ULr'^_DIS_pdu- >entity_veloci ty  .X  =  AUV_x_dot  *  METERS_PER_FT; 

UUV_DI£_pdu->enti  ty_veloci  ty  .y  =  AUV_y_dot  *  METERS_PER_FT; 

inTV_DI  £_i:>du-'>ent  1  ty_veloci  ty  .  2  =  AUV_2_dot  *  METERS_PER_FT; 

UUV_L’I  S_pdU“>dead_reckon_paraiTis  .  angular_veloci  ty  [  0  ]  = 

(int)  degrees  (AUV_phi_dot )  %  360; 

LTTjv_DIS_pdu->dead_reckon_params .  angular_velocity  1 1  ]  = 

(int)  degrees  (AUV_theta_dot )  %  360; 

injV_DIS_pdu->dead_reckon_parains  .  angular_velocity  [2]  = 

(int)  degrees  (AUV_psi_dot )  %  360; 

-' /  no  explicit  world  coordinate  accelerations  are  provided  by  dynamics  model 


UUV_M£_pdu- >dead_reckon_jparanis  .  iinear_accel  [Oj  =0.0; 

1  ^—pdu- ■-dead_reckon_params  .  1  inear_accel  [1]  =0.0; 

UUV_Dl£_pdu->dead_reckon_params . linear_accel  [2]  =0.0; 

debcg  code: 

LTry_b I  £_pdu  - >en  t  i  ty _ve  1  o  c  i  ty  .  x  =0.0; 

'jltv  _ I ' :  5- _pd u  -  ■  >en 1 1  r  y _v e  1  o c  i  t y  .  y  =0.0; 

■Jirv'_D I  £_pdu  -  ">en  1 1  ty_ve  1  o  c i  ty  .  z  =0.0; 


lp:'v'_r.I£_pdu->dead_reckon_params .  angular_velocity  [0]  =  0; 
:jUV_DI£_pdU“>dead_reckon_params . angular_velocity  [1]  =  0; 
UUV_DI£_pdu->dead_reckon_params . angular_veloci ty  [2]  =  0; 

(TRUE) 

( 

cout  <<■  endl; 

cout  «  •’ DI£_net_wri  te :  time  “  «  AUV_time  «  " ; 

cout  <<  ”  {  '■  ; 

ccut  <*"  UU'/_DIS__pdu->enti  ty_Iocation  .X  «  “/  “  ; 

cout  «  UUV_DIS_pdu->enti ty_location .y  «  ",  " ; 

cout  «  UU\^_DIS__pdu->enti  ty_location ,  2  «  ",  "; 

cout  UU^.'_DI£__pdu->enti  ty_orientation  .phi  «  ",  "; 

cout  UUV_DIS_pdu->enti  ty_orientation .  theta  «  ",  "; 

cout  •'<  UUV_DI£_pdu->enti  ty_orientation  .psi  «  "]"  «  endl; 

if  ^ TRACE} 

{ 

cout  «  "World  coordinate  velocities:  "; 

c Gu  t  UITv_D I  £_pdu  -  >en  t  i  ty _vel  oc  i  ty .  x  «  "  ; 

c  ou  t  <:<"  UW_r: I  S_pdu  -  '-en  t  i  ty_vel  oc  i  ty  . y  «  " ,  " ; 

cout  ULT^v^__DIS_pdu->en  ti  ty_veloci  ty  ,  z  «  ",  "; 

cout  UlTV__DIS_pdu->dead_reckon_params .  angular_velocity  [0]  «  ",  "; 
cout  «  Utn/_DI£_pdu->dead_reckon__params .  angular_velocity  [1]  «  ",  "; 
cout  «'  ULr'/_DIS_pdu->dead_reckon_params .  angular^velocity  [2]  «  "]" 

<'o  endl; 


if  (TRACE) 

{ 

cout  «  "World  coordinate  accelerations:  "; 
cout  «  "  [  “  ; 

cout  «  UUV_DIS__pdu->dead_reckon_params .linear_accel  [0]  «  •,  "; 
cout  «  UUV_DIS_pdu->dead_reckon_params . linear_accel  [1]  «  ",  ■; 
cout  «  UUV_DIS_pdu-'>dead_reckon_params  .linear_accel  [2]  «  "]"  «  endl; 

) 

//  what  we  look  like 

UUV_DI£_pdu->entity_appearance  =  AppearanceSubSurf^SmallWake; 

UUV_DI£_pdu->entity_marking, character_set  =  CharSet_ASCII; 

strncpy  ((char  *)  UUV_DIS_pdu->entity_marking. markings,  "NPS  AUV  ", 

MARKINGS_LEN) ; 
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ArciculatParamsNode  *  APNptr; 

//  articulated  parameters:  rudders,  planes,  test  with  execution  level  rpm, 
bd  rpiT 

/'/  note  that  the  function  specification  for  at tachArti culatParamsNode 
.  ,  yjt-eded  to  be  corrected  to  acceot  (char  *)  in  file  disdefs.h 


if  {APNptr  =  attachArticulatParamsNode  ( (char  *}  UUV_DIS_pdu, 

Ent ityStatePDU_Type ) ) 
i 

//  Successful  attach,  fill  in  data.  See  DIS  standard  paragraph  5.3.3 

APNptr->art iculat_params . change  =  TRUE;  //  always  active 

APNptr->art iculat_params . ID  =  0;  //  articulated  parameter  # 

APNptr->articulat_params .parameter_value  [0]  //  auv  time,  even  seconds 

=  (unsigned  short)  ((int)  AUV_time  %  600); 

APNptr->articulat_params .parameter_value  [1]  //  auv  time  tenths  of  seconds 

=  (unsigned  short)  (  AUV_time  *  10)  %  10; 

APNptr- >art iculat_params .parameter_varae  [2 ] 

=  (sianed  short)  ((int)  degrees  (AUV_delta_rudder ) 

%  360) ; 

Ar  Nrt  r-  '’art  icul  at_params  .  parameter_value  [  3  ] 

=  (signed  short)  ((int)  degrees  (AUV_delta_planes) 

%  360); 

APNpt r->articulat_params .parameter_value  [4 ] 

=  (signed  short)  ((int)  AUV_port_rpm  /  10); 

APNptr- oarticui at_params . parameter_value  [ 5 ] 

=  (signed  short)  ((int)  fabs  (AUV_port_rpm)  %  10); 

APNpt  r-  '.'art  icul at__xjarams  .  parameter_value  [  6  ] 

=  (signed  short)  {(int)  AUV_stbd_rpm  /  10); 

APNptr-:^articulat_jjarams  .parameter_value  [7  ] 

=  (signed  short)  ((int)  fabs  (AUV_stbd_rpm)  %  10); 

else  ccut  "[error  attaching  articulated  parameters  node  0:  time,  " 
"rudder,  planes,  rpm]"  «  endl  ; 

■'/  articulated  parameters:  Thruster  parameters 

rf  (APNptr  =  a tt acaArticulatParamsNode  ({char  *)  UUV_DIS_pdu, 

EntityStatePDU_Type) ) 


//  Succ 
APNptr- 
APNptr- 
APNpt r- 
APNptr- 
APNpt r- 
APNptr- 
APNptr- 
APNptr- 
APNptr- 
APNptr- 


esstu 

>arti 

>arti 

>arti 

>arti 

>arti 

>arti 

>arti 

>arti 

>arti 

>arti 


1  attach,  fi 
culat_params 
culat_params 
culat_params 
culat_params 
culat_j)arams 
culat_params 
culat_params 
culat_params 
culat_params 
culat_params 


11  in  data.  See  DIS  standard  paragraph  5.3.3 
.change  =  TRUE;  //  always  active 
.ID  =1;  //  articulated  parameter  # 


.parameter_value  [0] 
. parameter_value  [ 1 ] 
. pa ramete revalue  [2] 
.parameter_value  [3] 
.parameter_value  [4] 
.parameter_value  [5] 
.parameter_value  [6] 
. pa ramete revalue  [7] 


(int)  AUV_bow_ver t i c  a 1 ; 
(int)  AUV_stern_vertical  ; 
(int )  AUV_bow_lateral; 
(int )  AUV__stern_lateral ; 
0; 

0; 

0; 

0; 


else  cout  «  "[error  attaching  articulated  parameters  node  1:  thrusters] 
<<  endl ; 


//  art iculated’  parameters :  Sonar  parameters 

if  (APNptr  =  attachArticulatParamsNode  ( (char  *)  UUV_DIS_pdu, 

EntityStatePDU_Type) ) 

{ 

//  Successful  attach,  fill  in  data.  See  DIS  standard  paragraph  5.3.3 
APNptr->articulat_params . change  =  TRUE;  //  always  active 
APNptr->articulat_params . ID  =1;  //  articulated  parameter  # 

APNptr- >articul at _params .parameter^value  [0] 

=  (signed  short)  ((int)  AUV_ST1000_bearing)  /  10; 
APNptr->articulat_params .parameter_value  [1 ] 

=  (signed  short)  ((int)  AUV_ST1000_bearing)  %  10; 
APNptr->articulat_params .parameter_value  [2 ] 
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=:  (signed  short) 

{  (int)  (Airv_ST1000_rangG*4 .0  +  0,5)  )  ; 

APiN!ptr->articulat_parains  .paramGter_value  [3] 

=  (signed  short)  ((int)  AUV_ST1000_strength) ; 

APNpt  r--,'art iculat_params  . pa ramete revalue  [ 4  ] 

=  (signed  short)  ((int)  AUV_ST725_bearing)  /  10; 

APNptr->articulat_params .paraineter_value  [5 ] 

=  (signed  short)  {(int)  AUV_ST725_bearing)  %  10; 

APNptr->art  iculat^params  .paraTnGter_value  ( 6  ] 

=  (signed  short)  { (int ) (AUV_ST725_range*4 . 0+0 .5 ) ) ; 

APNptr->articulat_parains  .parameter_valuG  [7  ] 

=  (signed  short)  ((int)  AUV_ST725_strength) ; 

> 

else  ccut:  "[error  attaching  articulated  parameters  node  2:  sonar]" 

-'•r  endl ; 

//  Identify  dead  rec)<oning  algorithm;  world  coordinates,  accelerations  zero 

(Algorithm.  3) 

UIjV_L'I  S_pdu->dead_reclcon_params .  algori  thm  =  DRAlgo_DRM_RPW; 

//  send  out  the  multicast  PDU 

:nr  result  =  net^write  {(char  *)  UUV_DIS_pdu,  EntityStatePDU^Type) ; 

if  (result  ==  FALSE) 

cout  «  "DIS_net_write  ()  failed"  «  endl ; 

else  if  (TRACE)  cout  «  "DI£_net_wri te  ()  successful,  returning"  «  endl; 

f reeEnti tyStatePDU  (UUV_DIS_pdu) ;  //  essential  to  prevent  memory  leak 

//  articulated  parameters  are  also  freed 


return  resu] 


//  end  of  DI£_net__write  () 


void  DISNetworkedRlgidBody : :  DIS_net_close  () 

DI  £_port_oper]  =  FALSE; 
net^clcse 


void  l: SIvetworkedRigidBody : ;  DISNetworkedRigidBody_ini tialize  C 


P I  g  I  ut'  c: oy _  1  n  i  1 1  a  1 1  z e  ^  ;  ; 
time  of  last_PDU  =  0.0; 


//  inherited  method 


void  DISNetworkedRigidBody : :  set_timG_of_lasc_PDU 

(const  double  new_time_of_last_PDU) 


:imie_cf_last_PDU  =  new_time_of_last_PDU; 


Id  DISNetworkedRigidBody : :  set_ttl  (int  new_ttl) 


cout  «  "DISNetworkedRigidBody : :set_ttl :  old  ttl 


«  (int)  ttl; 


if  (new_ttl  >  16) 


cout  «  endl; 

cout  «  "A  large  ttl  value  may  impact  people  outside  your  network  “ 
«  endl; 

cout  «  "and  even  around  the  world!"  <<  endl; 
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cout  «  "Please  confirm  by  entering  the  new  time  to  live  (ttl)  " 

<<"  "value  f  *'  <•<  new__ttl  «  “)  yourself:  ; 

cin  »  new_Ltl; 
cout  • endl  ; 

'  ' r^ew^t 1 1  <  1  }  M  ( new_t 1 1  >  2  5  5)) 
cout  <■<  endl  ; 

cout  "Time  to  live  (ttl)  value  out  of  range  (l.,255),  ignored." 

«  endl ; 
return; 


ttl  =  (u_char)  new_ctl ; 

cout  <<  endl ; 
cout  <<  " 

«  (int)  ttl  «  endl; 


new  ttl 


v-.:c]  rdSNetworkedRigidEody :  :  set_group  (char  *  newsgroup) 
cout  <<  “DISNetworkedRigidBody : : set_group :  old  group  =  ' 


«  group; 


bzerc  (group,  20); 
s  t  r c  Y-'-Y  ( g  r o up,  n  e v.'_g  roup)  ; 


new  group  =  "  <<  group  «  endl ; 


void  DI SNetv;crkedKlgidBody :  :  set_pcrt  (char  *  new_port) 

cout  "DISNetworkedRigidBody  :  :  set_port :  old  port  =  "  <<  port; 


new_port ) ; 


new  port  =  “  «  port  «  endl ; 


tendif  //  DISNETWORKEDRIGIDBCDY  C 
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RigidBody.C  Rigid  Body 


'■/-■■/////////////////////////////////////////////////////////////////////////// 

Prog  rani:  RigidBody.C 

Author:  Don  Brutzman 

Revised:  18  October  94 

Sy .?  r  en'i :  I  r  i  x  5 . 2 

C  omp i i e  r :  ANSIC+  + 

Compilation:  irix'^  make  dynatest 

irix>  CC  RigidBody.C  -Im  -c  -g  +w 

-c  ==  Produce  binaries  only,  suppressing  the  link  phase. 

+w  ==  Warn  about  all  questionable  constructs. 

l;e:?cription :  Rigid  body  class  specification. 

Note:  world  coordinate  system,  NOT  body  coordinate  system, 
thus  Euler  angles  are  used. 

Advisers:  Dr.  Mike  Zyda ,  Dr.  Bob  McGhee  and  Dr.  Tony  Healey 

References:  Fu ,  K.S.,  Gonzalez,  R.C.,  and  Lee,  C.S.G., 

^Robotics:  Control,  Sensing,  Vision  and  Intelligence^, 

McGraw-Hill  Inc.,  NY,  1987. 

Cooke,  J.C.,  Zyda,  M.J.,  Pratt,  D.R.,  and  McGhee,  R.B., 

"NPSNET:  Flight  Simulation  Dynamic  Modeling  using 

Quaternions,"  _PRESENCE_,  vol .  1  no .  4,  Fall  1992, 
pp.  404-420. 

Cooke,  Joseph  C.,  "NPSNET:  Flight  Simulation  Dynamic 
Modeling  using  Quaternions,"  masters  thesis.  Naval 
Postgraduate  School,  December  1993. 


^  '  .'/^C//.C///,C/ //////////// ////////////////////////////////////// ///////// 

^:fndef  RIGIDBODY_C 

#define  RIGIDBODY^C  1 1  prevent  errors  if  multiple  #includes  present 

# include  "Hmiatrix.C" 

class  RigidBody 
/ 

private : 

//  member  data  fields 


double  time_of_posture; 

double  x_dot; 
double  y_dot; 
double  z_dot; 
double  phi_dot; 
double  theta_dot; 
double  psi_dot; 


//  linear  velocity  along  x  axis 
//  linear  velocity  along  y  axis 
//  linear  velocity  along  z  axis 
I (  angular  velocity  about  x  axis 
//  angular  velocity  about  y  axis 
//  angular  velocity  about  z  axis 


public : 
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If  need  to  access  print  &  set  routines 


Hmatrix  hmatrix; 

//  constructors  and  destructor 


RicidBody 

( )  ; 

RiqidBody 

( const 

Hmatrix  initialhmatrix) ; 

RigidBody 

(const 

Hmatrix  initialhmatrix,  const 

double  t ) ; 

PiqidEody 

(const 

double 

X, 

const 

double 

y. 

const 

double 

z. 

const 

double 

phi  , 

const 

double 

theta , 

- 

const 

double 

psi , 

- 

const 

double 

t)  ; 

~Ri gidBody 

0 

{  /*  null  body  */ 

} 

//  operators 

R:g:d3cdys 

operator  = 

( const 

RigidBody  &crb_rhs);  // 

assignment 

friend  ostrearm  &  operator  «:  (ostreamSc  out,  RigidBody&  rb)  ; 


//  inspection  methods 


void 

p  r  i  n  t  i g i dbcdy 

()  const ; 

rim  at  rix 

nmatrix^value 

0  const; 

double 

time_of__posture_value 

0  const; 

double 

x_value 

()  const; 

double 

y_value 

0  const; 

dcuiil  e 

z_value 

()  const; 

dcuitle 

pni_va  j-  ue 

0  const; 

dour  j.  e 

theta  _v  a 1 u  e 

()  const; 

double 

psi_value 

()  const; 

double 

x_dot_value 

()  const; 

double 

y_dot_valuG 

0  const; 

double 

z_dot_valuG 

{)  const; 

double 

phi_dot_value 

0  const; 

d  o  u  o  j.  e 

theta_dot_value 

0  const; 

doub I e 

psi_dot_value 

()  const; 

mod: tying 

iTie  t  h  cci  s 

void 

RiQidBod\'_ini  tialize 

0  ; 

void 

set_velocities 

(double  new_x_dot, 
double  new_j/_dot, 
double  new_z_dot, 
double  new_phi_dot, 
double  new_theta_dot , 
double  new_psi_dot} ; 

void 

set_linear_veloci ties 

(double  new_x_dot, 
double  new_y_dot, 
double  new_z_dot} ; 

void 

set_angular_velocities 

{double  new_phi_dot, 
double  new_theta_dot, 
double  new_psi_dot) ; 

void 

set_time_of_posture 

(double  new_time_of_posturG) 
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upda  t  e^Hiria  t  r  i  X 


(double  delta_t) 


I  i  ;  i }  i  i  i  i  !  I  n  !  i !  I  i  !  f  !  n  !  I !  !  f  !  i . 


constructor  methods 


RigidBody::  RigidBody  {)  //  default  constructor 
iuTiatrix.  ser_identi  ty  ( }  ; 


ti:Tie_of_pcsture 

=  0 . 0  ; 

x_dot 

=  L'  .  U  ; 

y_dot 

=  0.0; 

2_dot 

o 

o 

II 

phi_dot 

=  0.0; 

theta_dot 

=  0.0; 

psi_dot 

=  0.0; 

<ig:dBody::  RigidBody  {const  Hmatrix  initialhmatrix) 

hmatrix  =  Hmatrix  (initialhmatrix) ; 
time_of_postur€  =  0.0; 
x_dot  =  0.0; 

y_dot  =  0.0; 

2_dot  =  0.0; 

phi_dot  =  0.  Ga¬ 
rnet  a_dot  =  O.C; 

osi  dot  =  0.0; 


RigidBody::  RigidBody  (const  Hmatrix  initialhmatrix,  const  double  t) 


hiTiatrix  = 

Hmatrix 

( ini 

tirrie_cf_po 

s  t  u  r  e  = 

C  ; 

X— doc 

= 

0 . 0  ; 

y_dcc 

= 

0 . 0  ; 

z_dct 

= 

0.0; 

phi_dot 

= 

0.0; 

theta_dot 

= 

0.0; 

psi_dot 

= 

0.0; 

RigidBody::  RigidBody  (const  double  x, 

const  double  y, 
const  double  z, 
const  double  phi, 
const  double  theta, 
const  double  psi, 
const  double  t) 

hmatrix  =  Hmatrix  (phi,  theta,  psi,  x,  y,  z); 

time_of_posture  =  t; 

x_dot  =  0.0; 

y_dot  =  0.0; 

2_dot  =  0.0; 

phi_dot  =  0.0; 

theta_dot  =  0.0; 

psi_dot  =  0.0; 
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// 


dcubie  RiqidBody::  z_value  (} 


return  hmatrix . z_value  (}; 


i 

double  RigidBody::  phi_value  () 
const 

return  hmatrix . phi_value  (); 

/  /  ~ 

dcubie  RigidBody::  theta_value  () 
const 
{ 

return  hiriatrix  .  theta_value  (); 

double  RigidBody::  psi_value  () 
const 

return  hmatrix . psi_value  (); 

I 

double  RigidBody;:  x_dot_value  () 
c  0  n  s 

return  x_dot ; 

double  RigidBody:  :  y___dct_value  () 

return  y^dot; 

double  RigidBody : :  2_dot_value  () 

const 

/ 

return  z_dot ; 

double  RigidBody::  phl_dot_value  () 
const 
( 

return  phi_doC; 

} 

double  RigidBody : :  theta_dot_value  () 
const 
{ 

return  theta_dot; 

} 

- - ---// 

double  RigidBody::  psi_dot_value  () 
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return  psi_dot; 


H: t G I  f  y  I  n g  me  t  n oa s 


void  RloidEody::  Rigid&ody_ini tiali ze  (1 
{ 

hmatrix . set_identity  {); 


^_of_posture 

=  0.0; 

X  dot 

=  0.0; 

y_dot 

=  C.O; 

z  dot 

=  0.0; 

ii_dot 

=  0.0; 

:a_dot 

=  0.0  ; 

u_dot 

=  0.0; 

RigidBody::  set_veloci ties 


(double  new_x_dot; 
double  new_y_dot, 
double  new_z_dot , 
double  new^hi_dot, 
double  new_cheta_dot , 
double  new_psi_dot) 


X_dOL 

=  riew_x_dot; 

y_Go: 

=  n  t  V.- _ y _ G  O  _  ; 

Z_dOt 

=  new_z_dot; 

phi_dot 

=  new_phi_dot; 

thet  a_do t 

=  new_theta_dot 

p3 i_dot 

=  nev;_psi_dot ; 

jidBody  :  : 


x_dot  =  n€w_x_dot 
y_dot  =  new_y_doi; 
2  dot  =  new_z_dot 


_linear_veloci ties 

(double  new_x_dot, 
double  new_y_dot , 
double  new_z_dot ) 


void  RiqidBody::  set_angular_veloci ties 

(double  new_phi_dot, 
double  new_theta_dot , 
double  new_psi_dot) 

{ 

phi_dot  =  new_phi_dot; 
theta_dot  =  new_theta__dot  ; 
psi_dot  =  new_psi_dot; 


void  RigidBody::  set_time_of_posture  (double  new_tiine_of_posture) 


time_o  imposture  =  new_tiipe_of_posture; 


void  RigidBody::  upda te_Hinatrix  (double  delta_t) 
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hmatrix .  incremental_rotation  (  phi_dot:,  theta_dot,  psi_dot;  delta_t); 
hinacrix . incremental_translation  (Vector3D  {  x_dot,  y_dot,  z_doc) ^  deltaic); 
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Hmatrix.C  Homogeneous  Transform  Matrix  Mathematics 


/ ' i  : i ! ! : i :  i ! I i i ! I  U  i 1 1 ! 1 1 1 ! i i I i U i ! 1 1 ! U I ! N 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 ii ! 1 1 !  I !  I !!!  1 1 1 1 1 
Frograiri:  Hmatrix.C 

Author:  Don  Brutzman 


Revi sed : 


12  October  94 


System: 
Compiler : 
Compi la t i on : 


Description : 


References : 


Iris 

ANSI  C++ 

irix>  make  dynatest 

irix>  CC  Hmatrix.C  -Im  -c  -g  +w 

-c  ==  Produce  binaries  only,  suppressing  the  link  phase. 
tw  -=  Warn  about  all  questionable  constructs. 

Homogenous  transform  matrix  class  specification 
All  angle  parameter  values  are  in  radians 

rotations  are  in  world  coordinate  system 
translations  are  in  world  coordinate  system 

Dr.  Mike  Zyda,  Dr.  Bob  McGhee  and  Dr.  Tony  Healey 

Fu,  K.S,,  Gonzalez,  R.C.,  and  Lee,  C.S.G., 

^Robotics:  Control,  Sensing,  Vision  and  Intell igence_, 

McGraw-Hill  Inc.,  NY,  1987. 

Cooke,  J.C.,  Zyda,  M.J.,  Pratt,  D.R.,  and  McGhee,  R.B., 
"NPSNET:  Flight  Simulation  Dynamic  Modeling  using 

Quaternions,”  _PRESENCE_,  vol .  1  no.  4,  Fall  1992, 
pp.  404-420. 

Cooke,  Joseph  C.,  "NPSNET:  Flight  Simulation  Dynamic 
Modeling  using  Quaternions,  '*  masters  thesis.  Naval 
Postgraduate  School,  December  1993. 


////////////////////////////////////////,V/ ///////////////////////////////////// 
#ifndef  HMATRIX_C 

^^defrne  HM_ATR1X_C  //  prevent  errors  if  multiple  ^includes  present 
# include  "Quaternion , C” 

class  Hmatrix 
{ 

private : 

//  member  data  fields 

double  htmatrix  i4][4]; 
public : 

//  constructors  and  destructor 
Hmatrix  ( ) ; 

Hmatrix  (const  double  x, 

const  double  y. 
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const 

double 

z. 

const 

double 

phi , 

const 

double 

theta, 

const 

double 

psi)  ; 

Hn-amx 

(const 

Vector3D 

posit.ion_Vector3D, 

const 

double 

phi , 

const 

double 

theta, 

const 

double 

psi)  ; 

Hmatrix 

(const 

Vector3D 

position_Vector3D, 

const 

Vector3D 

eulerangles3D) ; 

Hmatr:>: 

(const 

Vector3D 

position_Vector3D, 

const 

Quaternion  posture) ; 

Hm.at  rix 

(const 

HmatrixSc 

input_hmatrix) ; 

-Hmatrix 

( ) 

{  /*  null  body  */  } 

operators 

hLtat  r:x& 

operator  = 

(const 

Hmatrix 

£ch_rhs)  ;  //  assignment 

Hm=a:rix^ 

operator  * 

(const 

Hmatrix 

&h_rhs) ;  //  Hmatrix  multiply 

Hmac  rrx& 

operator  * 

(const 

VectorSD 

Scv_rhs);  //  Vector3D  multiply 

friend  ost 

ream^  operator 

<<  (osi 

treamSc  os, 

Hmatrixfic  h) ; 

/ /  i n  3p  e c  L i on  methods 


void 

print_hmatrix 

()  const; 

double 

x_vaiue 

{)  const; 

dcunle 

y^value 

()  const; 

dourle 

Z  ^  V  g  1  Ufc 

( )  cons t ; 

douche 

ph revalue 

( )  const ; 

dcubitr 

theta_vaiue 

( )  const ; 

douD^e 

psi_value 

(}  const; 

Quaternion 

quaternion. 

.value 
( ) 

const; 

Veer  or 3D 

position 

{  ) 

const ; 

V-cr  or 3D 

camera 

( ) 

const ; 

Q  o  l1  e 

scale 

0 

const; 

double 

element 

(const  int 
const  int 

row, 

column) 


const; 


//  accessor  returns  lvalue 
//  for  row/column  [1..4] 


//  modifying  methods 


void 

rotate 

(const 

double 

phi , 

const 

double 

theta. 

const 

double 

psi)  ; 

void 

rotate 

(const 

Vector3D  rotation3D) 

void 

rotate_x 

(const 

double 

phi)  ; 

void 

rotate_y 

(const 

double 

theta) ; 

void 

rotate_z 

(const 

double 

psi)  ; 

void 

incremental. 

^rotation 

(const 

double 

phi_dot , 

const 

double 

theta_dot , 

const 

double 

psi_dot , 

const 

double 

delta_t  )  ; 
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void  translate  (const  double  delta_x, 

const  double  deita_y, 

const  double  delta^z); 

void  translate  (const  Vector3D  translation3D) ; 

void  incremental_translation 

{const  double  x_dot,  const  double  y_dot, 

const  double  z_dot,  const  double  delta_t) ; 

void  incremental_translation 

(const  VecCor3D  velocities3D, 

const  double  delta_t); 

void  set_identity  (); 

void  set_posture  (const  double  phi, 

const  double  theta, 
const  double  psi ] ; 

void  set_pcsition  (const  Vector3D  translation3D} ; 

void  set^camera  (const  Vector3D  translation3D) ; 

void  inove_carriera  (const  Vector3D  translation3D)  ; 

void  set_all_scales (const  double  scale_x, 

const  double  sealery, 

const  double  scale_z, 

const  double  scale  global ) ; 

void  set_3cale  (const  double  scale  global ) ; 


//  rotation  matrix  conventions:  Cooke  et  al.  Figure  10 


//  rotation  matrix 
Adeline  al  htma trix [ 0 ] [ 0 ] 

^define  a2  htma trix [1 ] [ 0 ] 

^define  a3  htmatrix [ 2 ] [ 0 ] 

#define  bl  htmatrix [ 0 ][ 1 ] 

^define  b2  htma trix [ 1 ][ 1 ] 

#define  b3  htmatrix [ 2 ][ 1 ] 

^fdefine  cl  htmatrix  [0  ][  2  ] 

tdefine  c2  htma trix [ 1 ]  [2 ] 

^define  c3  htmia trix  [ 2  ]  [  2  j 


//  position  translation  vector 
#define  p_x  htmatrix [0 ][ 3 ] 

#define  p_y  htma t rix [ 1 ] [ 3 ] 
^define  p_2  htmatrix [ 2 ]{ 3 ] 


//  camera  perspective  transformation 
#define  c_x  htmatrix [3 ][ 0] 

#define  c _y  htmatrix [3 ][ 1 ] 

#define  c_z  htmatrix [ 3 ]{ 2 ] 


//  global  scale  coefficient 
#define  hscale  htmatrix [ 3 ][ 3 ] 


// 
// 

Hmatrix::  Hmatrix  ()  //  default  constructor 


// - 

//  constructor  methods 
// - 
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} 

/  - - 

//  operators 

- - 

HiTiatrixSf  Hmatrix::  operator  =  (const  Hmatrix  &h_rhs)  //  assigninent 

f 

for  (int  row  =  0;  row  <=  3;  row++) 

^  for  (int  column  =  0;  column  <=  3;  column++) 

^  htmatrix  [row] [column]  =  h_rhs . htmatrix  [row] [column]; 


// 

// 
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Hniaci'ix^  Hmatrix::  operator  *  (const  Hmatrix  &ch_rhs)  //  Hmatrix  multiplication 
{ 

static  Hmatrix  hresult  =  Hmatrix  (}; 

int  row,  column,  index; 

for  (row  =  Q;  row  <=  3;  row++) 

for  (column  -  0;  column  <=  3;  column++) 
i 

hresult . htmatrix  [ row] [column]  =  0.0; 

) 

} 


] 


for  (row  =  0;  rov;  <=  3;  row-^+) 

for  iCGlumn  =  0;  column  <=  3;  columns +) 

for  (index  =  0;  index  <=  3;  index++) 

( 

hresul t . htmatrix  [row] [column]  =  hresult . htmatrix  [row] [column]  + 
htmatrix  [row] [index]  *  h_rhs .htmatrix  [index] [column] ; 

} 

) 

; 

return  hresult; 


// 


Hmati'lxc.'  Hmistrix:  :  operator  *  (const  Vector3D  &v_rhs)  //  Vector3D  multiply 

static  Hmatrix  hresult  =  Hmatrix  {); 
int  row,  columin; 


double  position. 

.vector  [4]; 

posi tion_vector 

[ 0 ]  =  v_rh  s 

[1] 

po3iticri_vector 

[i]  =  V  rhs 

[2] 

posi ti cn_vec tor 

[2]  =  V  rhs 

[3] 

posi tion_vector 

[3]  =  1.0; 

for  (row  =  0;  row  <=  3;  row++) 

{ 

for  (column  =  0;  column  <=  3;  column++) 

{ 

hresult .htmatrix  [ row] [column]  =  0.0; 

) 

} 

for  (row  =  0;  row  <-  3;  row++} 

{ 

for  (column  =  0;  column  <=  3;  column++) 

{ 

hresul t - htmatrix  [row] [column]  = 

htmatrix  [row] [column]  *  posi tion__vector  [column] • 

} 

) 

return  hresult; 


ostream&c  operator  <<  (ostreamSc  out,  Hmatrix&  h) 

{ 

i n  t  row ; 

for  [row  =  0;  row  <=  3;  row++) 

{ 

out  <<  endi  «  " [ “ 

«  h.htmatrix  [row][0)  «  “,  "  «  h.htmatrix  [row] [1]  «  *,  • 
«  h.htmatrix  [row] [2]  «  ",  "  «  h.htmatrix  [row] [3]  «  •]"; 

out  endi  ; 
return  (out); 

} 

/ 1 - 

//  inspection  methods 


vo:d  HiTiatrix::  print_hmat rix  () 


ii'iL  row; 

for  (row  =  0;  row  <=  3;  row++} 

( 

cout  --<•  endi  -<  •' [  *' 

<<-'  htmatrix  [row][0]  «  ",  "  «  htmatrix  [row][l]  «  ”,  “ 
<<  htmatrix  [row] [2]  «  ",  ”  «  htmatrix  [row] [3]  « 


// 

// 


double  Hmatrix::  phi^vaiue  ()  //  cooke  (8.17) 

const 

f 

double  theta  =  theta^value  (); 
dcuti^  result; 

If  '(theta  ==  radians  {  90.0;)  M  (theta  ==  radians  (-90.0))) 

cout  "phi_vaiue  ()  warning:  theta  ==  ”  «  theta  «  endi  ; 

I 

result  =  acos  (arcclamp  {c3  ./  cos  (theta)))  *  sign  (b3); 
return  result; 


- -  ^ 

double  Hm.atrix:  :  theta_value  ()  //  cooke  (8.14) 

const 
/ 

return  asin  (arcclamp  (-a3)); 

} 

- - - 

double  Hmatrix::  psi_value  ()  //  cooke  (8.16) 

c  on  s  t 

{ 


double  theta  =  theta_value  {); 
double  result; 

if  ((theta  ==  radians  (  90.0))  II  (theta  ==  radians  (-90.0))) 
cout  «  "psi^value  ()  warning:  theta  ==  "  «  theta  «  endi; 

) 
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result  =  acos  {arcclainp  (al  /  cos  (theta_value  (})))  *  sign  (a2); 
return  result; 


- j  . 

double  HfTiatrix::  x_value  () 
c  0  n  s  t 
{ 

return  p_x; 


// - 

double  Hmatrix: :  y_value  () 
const 
( 

return  p_y; 

) 


// 


- - // 

double  Hmatrix::  z_value  () 
const 


return 


P_-; 


// 


Quaternion  Hmatrix::  quaterni outvalue  () 
const 

qresult  -  Quaternion  (); 
qC,  ql,  q2,  q2 ; 

*  sqrt  (al  +  b2  +  c3  +  1); 


//  ^  shoemake  p.  25  3,  FUllDA  or  p.41  cooke 

return  qresult; 


) 

// - - 

VectorBD  Hmatrix::  position  () 
const 
{ 

return  Vector3D  (p_x,  p_y/  p_2 ) ; 

} 

- - - 

Vector3D  Hmatrix::  camera  {) 
const 
{ 

return  Vector3D  {c_x,  c_y,  c_z ) ; 

} 


Quaterni on 

d  G  u  b  ^  e 

qC  =  (0.5; 
qi  =  0.0; 
q2  =  0.0; 
Q-  =  0  .  U  ; 


// 


// 


double  Hmatrix;;  scale  {) 
const 
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return  hscale; 


double  Hmatrix: : 
const 


element  (const  int  row, 

const  int  column) 


V 

if  ((row  <  1)  I i  (row  >4)  M  (column  <  1) 


//  accessor  returns  value 
//  with  row/column  [1..4] 

!  i  (column  >  4 ) ) 


cout  «  “Warning:  Hmatrix .element  (“  «  row  <<  ” ,  “  «  column  « 
" ;  is  an  invalid  accessor  (only  1..4  allowed),  “  « 

"  returning  value  of  0,0"  <<  endl ; 


static  double  dummy _value  =  0.0; 
return  dummy _value; 


// 


return  htmatrix  [ row-1 ] i column-1 ] ; 


modifying  methods 


void  Hiriatrix:  :  rotate  (const  double  phi,  const  double  theta,  const  double  psi ) 

double  hrotation  i3]i3]; 
double  hresult  [3j[3]; 

int  row,  column,  index;  //  reference:  Cooke  et  al.  Figure  10 

double  sinphi  =  sin  (phi); 

double  cosphi  =  cos  (phi); 

double  sintheta  =  sin  (theta); 

double  costheta  =  cos  (theta); 

double  sinpsi  =  sin  (psi); 

double  cospsi  =  cos  (psi); 


hrotation 

[0]  [0]  = 

costheta 

* 

cospsi ; 

// 

al 

hrotation 

[  1  j  i  0  ]  = 

costheta 

★ 

sinpsi  ; 

// 

a2 

nrotation 

[2]  [0]  -  - 

sintheta ; 

// 

a3 

hrotation 

[  0  ]  [1  ]  = 

sinphi 

■* 

sintheta  * 

cospsi  -  cosphi 

*  sinpsi; 

// 

bl 

hrotation 

[1]  [1]  = 

sinphi 

* 

sintheta  * 

sinpsi  +  cosphi 

*  cospsi; 

// 

b2 

hrotation 

L  2  ]  [  1  ]  = 

sinphi 

■» 

costheta ; 

// 

b3 

hrotation 

[0]  [2]  = 

cosphi 

* 

sintheta  * 

cospsi  +  sinphi 

*  sinpsi; 

// 

cl 

hrotation 

[1]  [2j  = 

cosphi 

* 

sintheta  * 

sinpsi  -  sinphi 

*  cospsi; 

// 

c2 

hrotation 

[2]  [2]  = 

cosphi 

* 

costheta ; 

// 

c3 

for  (row  = 

f 

0;  row  <= 

2;  row  +  ^-) 

1 

for  (column  =  0; 

column  <= 

2;  column++) 

{ 

hresult  [row] [column]  =  0.0;  //  zero  accumulators  first 

for  (index  =  0;  index  <=  2;  index++) 

{ 

hresult  [row] [column]  =  hresult  [row] [column]  + 

htmatrix  [row] [index]  *  hrotation  [index] [column] ; 

} 

} 

} 

for  (row  =  0;  row  <=  2;  row++) 

( 

for  (column  =  0;  column  <=  2;  column++) 

{ 


// 

// 
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htmatrix  [row] [column]  =  hresult  [row] [column] ; 

} 

} 

} 

; , - // 

void  Hmatrix::  rotate  (const  Vector3D  rotation3D) 

{ 

rotate  (rotation3D  [1],  rotationBD  [ 2 ] , rotat ion3D  [3]);  //  note  not  012 

} 

.  i - If 

void  Hmatrix::  rotate_x  (const  double  phi) 
i 

double  hrotation  [3] [3]; 
double  hresult  [3][3]; 

int  row,  column,  index; 

for  (row  =  0;  row  <=  2;  row++) 

for  (column  =  0;  column  <=  2;  column-+ } 

I 

if  (row^  ==  column)  hrotation  [row] [column]  =  1.0; 
else  hrotation  [row] [column]  =  0.0; 


i  1 ]  [  1 ]  =  cos  (phi ) ; 

i 2  i  [ 2  ‘  =  cos  (phi }  ; 

[2  ]  [  1 ]  =  sin  (phi) ; 

[1] [2]  =  -  sin  (phi) ; 

for  {row  =  0;  row  <=  2;  row++) 
i 

for  (column  =  0;  column  <=  2;  column++ ) 

hresult  [ row] [ column ]  =  0.0;  //  zero  accumulators  first 

for  (index  =  0;  index  <=  2;  index++) 

{ 

hresult  [row] [column]  =  hresult  [row] [column]  + 

htmatrix  [row] [index]  *  hrotation  [ index] [column]; 


hrotation 
nrotat ion 
hrotation 


(row  =  0;  row  <=  2;  row++) 

for  (column  =  0;  column  <=  2;  column++} 

{ 

htmatrix  [ row] [ column]  =  hresult  [row] [column] ; 

} 


void  Hmatrix::  rotate_y  (const  double  theta) 
{ 

double  hrotation  [3][3]; 
double  hresult  [3][3]; 


int  row,  column,  index; 


for  (row  =  0;  row  <=  2;  row++) 

{ 

for  (column  =  0;  column  <=  2;  column++) 


if  (row  ==  column)  hrotation  [row] [column]  =  1.0; 
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else 


hrotation  [row] {column]  =  0.0; 


) 

hrotacion  [0] [0] 
hrot at i on  [2]  [2] 
hrotation  [2] [0] 
hrotation  [0] [2] 


cos  (theta) ; 
cos  (theta) ; 
sin  (theta); 
sin  (theta)  ; 


for  (row  =  0;  row  <=  2;  row++) 

/ 

for  (column  =  0;  column  <=  2;  column++) 

'  hresult  [row] [column]  =  0.0;  //  zero  accumulators  first 

for  (index  =  0;  index  <=  2;  index++) 

hresult  [row] [column]  =  hresult  [row] [column]  _ +  .  ,  , 

htmatrix  [row] [index]  *  hrotation  [index] [column]; 

} 

} 

for  ‘  rcvv  =  (i ;  row  •' =  2;  row-r  +  j 

foi  (cclumn  =  0;  column  <=  2;  column-t-+) 

htmatrix  [ row] [column]  =  hresult  [ row] [column] ; 


___// 


d  ^matrix::  rctate_z  (const  double  psi) 

doublrr  hrotation  [3]  [3]; 
dcutle  hresult  [3]  [3]; 

int  row,  column,  index; 

for  (row  =  0;  row  <=  2;  row++) 

for  (column  =  0;  column  <=  2;  columns + } 

if  (row  ==  column)  hrotation  [row] [column]  =  1.0; 
else  hrotation  [row] [column]  =  0.0; 


) 

hrotation  [0] [0] 
hrotation  [ 1 ] [ 1 ] 
hrotation  [  1 ] [ 0 ] 
hrotation  [C][l] 


cos  (psi 1 ; 
cos  (psi ) ; 
sin  (psi) ; 
sin  (psi); 


for  (row  =  0;  row  <=  2;  row-t-  +  ) 

^  for  (column  =  0;  column  <=  2;  column++) 

^  hresult  [row] [column]  =  0.0;  If  zero  accumulators  first 

for  (index  =  0;  index  <=  2;  index++) 

^  hresult  [row] [column]  =  hresult  [row] [column]  + 

htmatrix  [row] [index]  *  hrotation  [index] [column]; 


} 

} 

) 

for  (row  =  0;  row  <=  2;  row++) 

for  (column  =  0;  column  <=  2;  column++) 
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htmatrix  [row] [column]  =  hresult  [row] [column]; 


_  -  -  - ^  ^ 

void  Kmacrix::  incremental_rota tion  (const  double  phi_dot, 

const  double  theta_dot, 

const  double  psi_dot, 

^  const  double  delta_t  ) 

rotate  (pni_doc  *  delta_t,  theta_dot  *  delta_t,  psi_dot  *  delta_t) ; 

void  Hjmatrix::  translate  (const  double  delta_x, 

const  double  delta_y, 

^  const  double  delta_2} 

P _ ^x  +  =  de  1 1  a _ X ; 

+=  delta_y; 
p_z  +=  delta_z; 

) 

//_ _ _ 

voio  Rmatrix;:  translate  (const  Vector3D  translation3D) 

p_x  =  t  r a n  s  I  a  L  i  on  J  r  [  i  ]  ; 

transl at ion3D  [2]; 
transl  at  ion3D  [3]; 


- - 

vcio  Hniatrix::  incremental_t  ransl  at  ion  (const  double  x_dot, 

const  double  y_dot, 
const  double  z_dot, 
const  double  delta_t) 

1-=  x_dot  *  delta_t; 

y_dot  *  delta_t; 

-^=  z_dot  ^  delta_t; 


.  ^  ^ 

void  Hmatrix::  incremental_translat ion  (const  Vector3D  veloci ties3D, 

const  double  delta  t) 
i 

P_x  velocities3D  [1]  *  delta_t; 
p_y  +=  velocities3D  [2]  *  delta_t; 
p_z  -i-=  velocities3D  [3]  *  delta_t; 

} 


// 


--// 


void  Hmatrix: : 
{ 


set_identity  ()  //  default  constructorset  to  identity  matrix 


for  (int  row  =  0;  row  <=  3;  row+4-) 

{ 

for  (int  column  =  0;  column  <=  3;  column++] 
f 

if  (row  ==  column)  htmatrix  [row] [column]  =  1.0; 
else  htmatrix  [row] [column]  =  0.0; 
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#undef  al 
#undef  a2 
#undef  33 
#undef  bl 
#undef  b2 
#undef  b3 
#undef  cl 
#undef  c2 
#undef  c3 
tundef  p_x 
#undef  p_y 
tundef  p_z 
tundef  c_x 
tundef  c_y 
tundef  c_2 


#unde 
/  /  /  •  / 
#endi 


=  f  hscale 

//////////////////// /////////////////////////////////////////////////////// 
f  //  #lfndef  HMATRIX^C 
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Quaternion.C  Quaternion  Mathematics 


I ! t  :! I !! II I ! I II I ! i ! I ! I ! I ! I ! I II I ! I ! I i ! I ! I ! I ! I i ! I ! I ! I ! 1 1 ! I ! I ! I ! I ! I II II II I ! I n II I ! I 

/* 

Program:  Quaternion.C 

Revisions:  Don  Brutzman 

Revised:  29  SEP  94 

System:  Irix 

CoHsp-ier:  ANSI  C+  + 

Compilation:  irix>  make  dynatest 

irix>  CC  Quaternion.C  -Im  -c  -g  +w 

-c  ==  Produce  binaries  only,  suppressing  the  link  phase. 

==  Warn  about  all  questionable  constructs. 

:.'e.?:riptior. :  Quaternion  class  specifications  and  implementation 

All  angle  pararrieter  values  are  in  radians. 

Advisors:  Dr.  Mike  Zyda,  Dr.  Bob  McGhee  and  Dr.  Tony  Healey 

References:  Haynes,  Keith,  “Computer  Graphics  Tools  for  the 

Visualization  of  Spacecraft  Dynamics, “  masters  thesis. 

Naval  Postgraduate  School,  December  1993.  Includes 
source  code  used  for  initial  version  of  quaternion. c 

Cooke,  J.C.,  Zyda,  M.J.,  Pratt,  D.R.,  and  McGhee,  R.B., 

•'NPSNET:  Flight  Simulation  Dynamic  Modeling  using 

Quaternions,"  _PRESENCE_,  vol .  1  no.  4,  Fall  1992, 
pp.  404-420. 

Cooke,  Joseph  C.,  "NPSNET:  Flight  Simulation  Dynamic 
Modeling  using  Quaternions,"  masters  thesis,  Naval 
Postgraduate  School,  December  1993. 

Chou,  Jack  C.K.,  "Quaternion  Kinematic  and  Dynamic 
Differential  Equations,"  IEEE  Transactions  on  Robotics 
and  Automation,  vol.  8  no .  1,  February  1992,  pp. 53-64. 

Funda,  Janez,  Taylor,  Russell,  and  Paul,  Richard  P., 

"On  Homogenous  Transforms,  Quaternions,  and  Computational 
Efficiency,"  IEEE  Transactions  on  Robotics  and  Automation, 
vol.  6  no.  3,  June  1990,  pp.  382-388. 

Shoemake,  Ken,  "Animating  Rotation  with  Quaternion  Curves," 
Association  for  Computing  Machinery  _SIGGRAPH_  Proceedings, 
vol.  19  no.  3,  July  22-26  1985,  pp.  245-254. 

//////////////////////////////////////////////////////////////////////////////// 
# i f  nd  e  f  QUATERN I ON_C 

tdefine  QUATERNION_C  //  prevent  errors  if  multiple  #includes  present 

#include  "VectorSD.C" 

#define  PI  3.1415926535897932 

#ifdef  TRUE 
#undef  TRUE 
#endif 
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=p:fdef  FALSE 
^^undc'f  FALSE 
#endi  f 

# define  TRUE  1 
^define  FA^LSE  0 

/////,U'// /////////////////////////////////////////////////////////////////////// 


//  util 

iry  function 

prototypes 

double 

sign 

(double 

x)  ; 

double 

degrees 

(double 

X)  ;  //  radians 

input 

double 

radians 

( double 

X) ;  //  degrees 

input 

doucl e 

a reel amp 

(double 

X)  ; 

double 

dnormalize 

(double 

angle_radians) ; 

//  returns 

0. 

.2PI 

int 

i norma 1 i ze 

(double 

angle_radians) 

//  returns 

0. 

.359 

{ return 

(int)  (degrees 

(angle_radians ) 

+  0 

////// /////// ////////////////////////////////////////////////////////// ///////// 
Cj.ass  Qu a  c. 0 I'm  on 
private ; 


rrierTiOei’  data 

fields 

G  0  u  b  I  e 

qO; 

d  O  U  b'  -L  0 

ql; 

double 

q2; 

dcunle 

q3; 

puiiUic : 


cons^roctcrs  and  destructor 


Qu  a  1 0  r  n i on 
Quaternion 


Quaternion 


Quaternion 
-Quaternion { ) 


//  operators 


Quaternion& 

Quaternion 

Quaternion 

Quaternion 

Quaternion 

doubled 


operator  = 
operator  + 
operator  - 
operator  * 
operator  * 
operator  [] 


(const  double  phi , 
const  double  theta, 
const  double  psi); 

(const  double  new_qO, 
const  double  new_ql, 
const  double  new_q2, 
const  double  new_q3); 

( const  Quaternions  q) ; 

{  /*  null  body  */  } 


(const  Quaternions  q__rhs); 
(const  Quaternions  q_rhs}; 
(const  Quaternions  q_rhs); 
(const  Quaternions  q_rhs); 
(double  scalar) ; 

(int)  ; 


friend  ostreams  operator  «  (ostreams  out,  Quaternions  q) ; 


250 


/ /  1 nspec  c i on  met  hods 

void  print  {); 


double 

phi_varue 

()  const 

double 

theta_value 

{ )  const 

double 

psi_value 

()  const 

double 

magni tude 

( )  const 

Vector3D 

euler_angles 

(}  const 

Quaternion 

conjugate 

0  const 

Quaternion 

inverse 

{ )  const 

//  Euler  angle  roll 
//  Euler  angle  pitch 
//  Euler  angle  yaw 

//  Normally  1.-0/  unit  quaternions 
//  Euler  angle  phi,  theta,  psi 
//  (roll,  pitch,  yaw) 

//  more  efficient  than  individual  calls 

//  negates  vector  part 

//  negates  vector  psrt  &  normalizes 


modifying  methods 

void  rotate  (const  double  deltajhi, 

const  double  delta_theta, 
const  double  delta_psi  ); 

void  iricreinental_rotate  (const  double  P,  const  double  Q, 

const  double  R,  const  double  delta_t); 


upoa  te 

(double 

P, 

double 

Q, 

double 

double 

R, 

seconds ) 

set 

ncnrial  ize 

(double 

/  ;  ; 

phi , 

double 

theta, 

double 

double 

psi, 

rotation 

,  .  ,  ■  ,  .  i  ;  !  :  ; :  !  I !  n  !  I !  i  !  I  i  !  i  1 !  n  !  I !  !  I !  I !  I  !  I !  I !  I !  I !  I !  I !  I !  I !  I !  I !  I !  !  I !  '  !  I  ^  ‘ 

double  sign  (double  xi 

if  (X  >  0 . 0 )  return  1.0; 

else  if  (X  <  0.0)  return  -1.0; 
else  return  1.0; 


double  degrees  (double  x)  //  radians  input 
return  x  18C.0  /  PI; 


double  radians  (double  x)  //  degrees  input 
{ 

return  x  *  PI  /  180.0; 

} 

- - 

double  arcclamp  (double  x) 
i 

if  (X  >  1.0) 

{ 

X  =  1.0; 

//  cout  «  •*  arcclamp  reduced  "  «  x  <<  “  to  1.0“  «  endl; 

} 

else  if  (X  <  -1 .0) 

{ 


// 


// 
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X  =  -  i  .  0  ; 

/ /■  ccut  <<  "  arcclamp  raised  ”  «  x  «  "  to  -1.0”  «  endl  ; 

return  x; 

)  - - // 


double  dncrrr.ali ze  (double  angle_radians ) 

double  new_angle  =  angle__radians ; 

while  {new_angle  >  2*PI)  new_angle  -=  2*PI; 
while  {new_angle  <0.0  )  new_angle  +=  2^ PI; 
return  new_angle; 

) 

// - // 

//  constructor  methods 

- // 

Quaternion::  Quaternion  () 

{ 


qO  =  1  . 0  ; 
ql  =  0 . 0  / 
q2  =  0.0; 
q3  =  0.0; 

} 

H - // 


iternion 

: :  Quaternion 

( const 

double  phi , 

const 

double  theta, 

const 

double  psi ) 

!!  reference:  Cooke 

thesis 

p .  41 

double 

r,  p,  y,  cosr, 

cosp, 

cosy,  sinr,  sinp,  siny; 

r  = 

(phi  /  2.0); 

P 

=  (theta  /  2.0) 

y  =  (psi 

/  2.o: 

cosr  = 

CCS  { r) ; 

cosp 

=  cos  (p) ; 

cosy  =  cos 

(y) ; 

sen  ir); 

sinp 

^  sin  (p) ; 

siny  =  sin 

(y) ; 

(COST  *  cost  * 

cosy ; 

+  (Sinr  *  sinp 

*  siny); 

u-  = 

(senr  *  cosp  * 

cosy ) 

-  icosr  *  sinp 

*  siny); 

q^  = 

(cosr  *  sinp-  * 

cosy ) 

+  (sinr  *  cosp 

*  siny) ; 

q3  =  - 

(sinr  *  sinp  * 

cosy ) 

+  (cosr  *  cosp 

*  siny) ; 

iternion 

: :  Quaternion 

{ const 

double  new_q0. 

const 

double  new_ql. 

const 

double  new_q2 , 

const 

double  new_q3 ) 

{ 

qO  =  new_q0; 
ql  =  new_ql ; 
q2  =  new_q2 ; 
q3  =  new_q3 ; 

} 

// - 


Quaternion: :  Quaternion 
{ 

qO  =  q.qO; 
ql  =  q-ql; 
q2  =  q.q2; 
q3  =  q.q3; 

) 

// - 

//  operators 


(const  Quaternion&  q) 


// 


// 


// 
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// 


Quaternions:  Quaternion:  :  operator  =  (const  Quaternion&  q_rhs) 

qO  =  q_rhs.qO; 
ql  =  q_rhs.ql; 
q2  =  q_rhs.q2; 
q3  =  q_rhs.q3; 
return  *this; 


Quaternion  Quaternion::  operator  +  (const  Quaternion&  g  rhs ) 
{ 

static  Quaternion  sum; 

sum.qO  =  qO  +  q^rhs.qO; 
sum.q:  =  ql  +  q_rhs.ql; 
sum . q2  =  q2  +  q_rhs . q2 ; 
sum.  q3  =  q3  -r  q_rhs .  q3  ; 
return  sum; 


aternior.  Quaternion::  operator  ■ 

static  Quaternion  difference; 

dif ference.qO  =  qO  -  q_rhs.qO; 
difference. ql  =  ql  -  q_rhs.ql; 
d: f f erence . q2  =  q2  -  q_rhs.q2; 
ai f ference .qS  =  q3  -  q_rh3.q3; 
reti.rn  difference; 


(const  Quaternions:  q_rhs) 


1  Quaternion::  operator  *  (const  Quaternions:  q_rhs) 
Quaternion  prod; 


prod.qO  = 

(qO 

* 

q_rhs.q0) 

- 

(ql 

X 

q_rhs .  ql )  - 

(q2 

* 

q_rhs . q2 ) 

- 

(q3 

* 

q_rhs  .q3 )  ; 

prod.ql  = 

(ql 

* 

q^rhs.qO) 

+ 

(qO 

* 

q_rhs . ql )  - 

(q3 

* 

q_rhs . q2 ) 

+ 

(q2 

★ 

q_rhs.q3)  ; 

prod.ql  = 

(q2 

* 

q_rhs.q0) 

+ 

(q3 

* 

q_rhs.ql)  - 

(qO 

* 

q_rhs . q2 ) 

4- 

(ql 

X 

q__rhs  .  q3  )  ; 

prod.ql  = 

(q3 

* 

q_rhs . qO ) 

+ 

(q2 

★ 

q_rhs.ql)  - 

(ql 

* 

q_rhs . q2 ) 

+ 

(qO 

X 

q_rhs . q3 } ; 

prod ; 


Quaternion  Quaternion::  operator  *  (double  scalar) 
{ 

static  Quaternion  product; 

product. qO  =  qO  *  scalar; 

product. ql  =  ql  *  scalar; 

product. q2  =  q2  *  scalar; 

product. q3  =  q3  *  scalar; 

return  product; 


doubled  Quaternion::  operator  []  (int  n) 
I 

if  (n  ==  0) 
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return  qC ; 


return  ql ; 


if  fn  ==  2) 


return  q2 ; 


return  q5 ; 


cout  «  "Warning:  quaternion ["  <<  n  «  "]  is  an  invalid  accessor" 
<<-  "  {only  0..5  allowed),  returning  value  of  0.0"  <<  endl  ; 
static  double  duinrTiy_value  =  0,0; 
return  dummy^value ; 


)Streani^  operator  <<  (ostream^  out,  Quaternions^  q) 

out  u-,'  "[“  <<  q.qO  <<  ",  "  <<  q.ql  <<  ",  " 

<<  q.q2  «  ",  "  «  q.q3  «  "]"  ; 
return  (out); 


nspection  met nods 


yjaternion::  print  i\ 


«  qU  << 
<<  q2  << 


"  «  ql  « 
"  <<  ql  << 


Quaternion::  phi_value 


//  Euler  angle  roll 


return  acos  {arcclairip  (  (q0*q0  -  ql*ql  -  q2*q2  +  q3*q3) 

/  cos  (theta_value  ()))) 

*  sign  (q2*q3  ^  q0*ql); 


double  Quaternion::  theta_value  ()  //  Euler  angle  pitch 

const 

{ 

return  asin  (arcclamp  (-2.0  *  {ql*q3  -  q0*q2))); 

} 

// - 

double  Quaternion::  psi_value  {}  //  Euler  angle  yaw 

const 

{ 

return  acos  (arcclamp  ( (qO^qO  +  ql*ql  -  q2*q2  -  q3*q3) 

/  cos  (theta_value  ()))) 


*  sign  (ql*q2  +  q0*q3); 
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double  Quaternion::  magnitude  () 


const 

return  sqrt ( {qO  *  qO )  +  (ql  *  ql )  +  (q2  *  q2 }  +  (q3  *  q3 ) ) ; 


VectorSD  Quaternion::  euler_angles  {}  //  Euler  angle  phi,  theta,  psi 

//  (roll,  pitch,  yaw) 

//  more  efficient  than  individual  calls 

const 

( 

double  qqO ,  qql ,  qq2 ,  qq3,  phi,  theta,  psi,  costheta;  //  locals  hide  methods 

qqO  =  qO  *  qO ;  //  force  optimization  of  squaring  computations 

qql  =  ql  *  ql ; 

qql  =  q2  *  q2; 

qq3  =  q3  *  q3 ; 

theta  =  asin  (arcclamp  (-2.0  *  (ql*q3  -  q0*q2))); 
costheta  =  cos  (theta) ; 

phi  =  acos  (arcclamp  ( (qqO  -  qql  -  qq2  +  qq3 )  /  costheta)) 

sign  (q2*q3  q0*ql); 

psi  =  acos  (arcclamp  ( (qqC  +  qql  -  qq2  -  qq3 )  /  costheta)) 

*  sign  (ql*q2  +  q0*q3); 
return  Vectcr3D  (phi,  theta,  psi ) ; 

/  _ _ _ 

Quaternion  Quaternion::  conjugate  ()  //  negates  vector  part 

const 

{ 

return  Quaternion  (qO,  -  ql ,  -  q2,  -  q3 ) ; 

) 


Quaternion  Quaternion::  inverse  ()  //  negates  vector  part 


static  Quaternion  qresult; 

qresult  =  conjugate  (); 
qresul t . normal ize  {); 
return  qresult; 


//  modifying  methods 

- - // 

void  Quaternion::  rotate  (const  double  delta_phi, 

const  double  delta_theta, 
const  double  delta_psi  ) 

Quaternion  rotation; 

rotation. q0=  -0.5* ((ql  *  delta_phi)  + (q2  *  delta_theta) + (q3  *  delta_psi)); 

rotation. ql=  0.5* ((qO  *  delta_phi)  + (q2  *  delta_psi)  -(q3  *  delta_theta ) ) ; 

rotation. q2=  0.5*({q0  *  del ta_theta ) + (q3  *  delta^phi)  - (ql  *  delta_psi)); 


rotation. q3=  0.5*  ((qO  *  deltajsi;  +  (ql  *  delta_theta )  -  (q2  *  delta_phi}}; 


qC  +=  rotation.qO; 
ql  -r=  rota t  i  on  .  ql  ; 
q2  rctarion.q2; 
q3  *=  rotation. q3; 
ncrnial  i  ze  ( ;  ; 
return; 

) 


// 


void  Quaternion::  increiriental^rotate  (const  double  P,  const  double  Q, 

const  double  R,  const  double  delta_t) 


Quaternion  rotation; 


rotation.qO  = 

-0.5 

*  delta. 

-t  * 

(  (ql 

"  P) 

+ 

(q2 

*  Q)  +  (q3 

*  R)) 

rotation.ql  = 

0.5 

*  delta. 

_t  * 

{  (qO 

*  P) 

+ 

(q2 

*  R)  -  (q3 

*  Q)  ) 

rotation.q2  = 

0.5 

*  delta. 

_t  * 

(  (qO 

*  Q) 

+ 

(q3 

*  P)  -  (ql 

*  R)  J 

rotation.ql  = 

0.5 

delta. 

_t  * 

(  (qO 

*  R) 

+ 

(ql 

*  Q)  -  (q2 

*  P)  ) 

qO  +=  rotation.qO; 
ql  +=  rota t ion. ql; 
q2  +=  rotation.q2; 
q3  rotation. q3; 
n  c  nTi  a  1 1 2  e  { )  ; 
return ; 

1 

/  / - II 


void  Quaternion::  update  (double  F,  double  Q,  double  R,  double  seconds) 
{ 


double  hh 

=  seconds  *  0 . 

5; 

douoie  h6 

=  seconds  /  6 . 

0; 

Quaternion  y 

=  *thiS;  dym, 

dyt,  yt. 

dydx; 

dyd>:.qO  =  -0.5 

*  ( iql  *  P)  - 

(q2  ’  Q) 

+ 

(q3 

*  R)  )  ; 

dydx.ql  =  0.5 

*  ( (qO  *  P)  + 

{q2  »  R! 

- 

(q3 

’  Q)  )  ; 

dya>:.q2  =  0.5 

*  { (qO  *  Q)  + 

(q3  ’  P) 

- 

(ql 

*  R)  )  ; 

dy'd>:.q3  =  0.5 

*  { (qO  *  R)  - 

(ql  *  Q) 

- 

(q2 

*  P)  )  ; 

yt  .qO  =  y .qO  ^ 

hn  dydx.qO; 

y t . ql  =  y . ql  + 

hn  *  dydx.ql; 

yt:.q2  =  y.q2  + 

.hn  dyax.q2; 

y t . q3  =  y . q3  ^ 

hh  *  dydx.ql; 

dyt.qO  =  -0,5 

*  (  (yt.ql  *  P) 

+  (yt.q2 

* 

Q)  + 

(yt.q3 

* 

R)  )  ; 

dyt.ql  =  0.5 

*  {  (yt  .qO  *  P) 

+  (yt.q2 

■* 

R)  - 

(yt.q3 

* 

Q)  )  ; 

d\^t.q2  =  0.5 

*  (  (yt .qO  *  Q} 

f  (yt.q3 

■* 

P)  - 

(yt .ql 

•St 

R)  )  ; 

dy t . q3  =  0.5 

*  (  (yt .qO  *  R) 

+  (yt.ql 

★ 

Q)  - 

(yt.q2 

St 

P)  )  ; 

y t . qO  =  y . qO  + 

hh  *  dyt.qO; 

yt.ql  =  y.ql  + 

hh  *  dyt.ql; 

yt.q2  =  y.q2  + 

hh  *  dyt.q2; 

yt.q3  =  y.q3  + 

hh  *  dyt.q3; 

dym.qO  =  -0.5 

*  (  (yt.ql  *  P) 

+  (yt.q2 

* 

Q)  + 

(yt.q3 

* 

R)  ) ; 

dym.ql  =  0.5 

*  (  (yt .qO  *  P) 

+  (yt.q2 

* 

R)  - 

(yt.q3 

* 

Q) )  ; 

dyin.q2  =  0.5 

*  (  (yt  .qO  *  Q) 

+  (yt.q3 

★ 

P)  - 

(yt.ql 

* 

R)  }  ; 

dym.qS  =  0.5 

*  (  (yt.qO  *  R) 

+  (yt.ql 

★ 

Q)  - 

(yt.q2 

* 

P)  )  ; 

y t . qO  =  y ■ qO  + 

seconds  *  dym 

-qO; 

yt.ql  =  y.ql  + 

seconds  *  dym 

.ql; 

yt.q2  =  y.q2  + 

seconds  *  dym 

.q2; 

yt.q3  =  y.q3  + 

seconds  *  dym 

-q3; 

dyit.qO  =  dyin.qO  +  dyt.qO; 
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dym 

ql 

dym . ql 

-»■  dyt.ql; 

dym 

q2 

= 

dym . q2 

+  dyt.q2; 

dvmi 

q3 

= 

dym .q3 

dyt.q3; 

dy : 

rfi 

-  C;  .  5  * 

(  (yt .ql  * 

P; 

+ 

{yt 

q2 

*  Q) 

(yc.q3 

*  R) 

dy  t 

ql 

0.5  * 

{  (yt .qO  * 

P) 

+ 

(yt 

.q2 

*  R) 

-  (yt.q3 

*  Q) 

dy  t 

q2 

= 

0.5  * 

( (yt .qO  * 

Q) 

+ 

(yt 

.q3 

*  P) 

-  (yt.qi 

*  R) 

dy  t 

q3 

= 

0.5  * 

(  (yt -qO  * 

R) 

+ 

(yt 

.ql 

*  Q) 

-  (yt.q2 

*  P) 

qO  = 

=  y 

qO 

+  h6  * 

(dydx . qO 

+ 

dy  t 

■  qO 

+  2 

.0  * 

dym.qO) ; 

ql  = 

=  y 

ql 

+  h6  * 

(c^dx  .ql 

+ 

dy  t 

•ql 

+  2 

.0  ♦ 

dym . ql )  ; 

q2  = 

=  y 

q2 

+  h6  * 

(dydx .  q2 

+ 

dy  t 

.q2 

+  2 

.0  * 

dym . q2 )  ; 

q3  = 

=  y 

q3 

e  h6  * 

(dydx .q3 

+ 

dy  t 

.q3 

+  2 

.0  * 

dym.q3)  ; 

normal . 

.ze 

1.)  ; 

d  Qua tern 

.ion :  ;  set 

(double  phi 

,  double 

qC  = 

cos 

(0.5  * 

twist)  ; 

ql  =  cos 

(phi!  * 

sin 

(0.5  * 

twist)  ; 

q2  =  cos 

(cneta)  * 

sin 

(0.5  * 

twist)  ; 

q3  =  cos 

(psi!  ’ 

sin 

(0.5  * 

twist)  ; 

) 


theta,  double  psi,  double  twist) 


// 


void  Quaternion;:  norinalizeO 
double  n-.  =  magnitude  (); 


(m  > 

0 

.0) 

/ 

mi  ; 

ql 

rri; 

ql 

/  ~ 

m  ; 

q3 

/  = 

m; 

/////// ////// //////////////////////////////////////////////////////// /////////// 
#endif  //  =jtifndef  QUATERNION_C 
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K. 


VectorSD.C  3D  Vector  Mathematics 


/  •,  .  .'///. '//.'/ /////// ////////////////////////// //////////////////// /////////// 

/  * 

Proqrani;  VectorSD.C 

Original  Author:  Keith  Haynes 

Revisions:  Don  Brutzman 

Revised;  12  FEB  94 

Systeir::  Iris /PC 

CorTip  i  1  e r :  ANS I  C+  + 

Compilation:  irix>  CC  Vector3D.C  -Im  -c  -g  +w 

-c  ==  Produce  binaries  only,  suppressing  the  link  phase. 

-(-w  ==  Warn  about  all  questionable  constructs. 

Description:  Vector3D  class  specifications  and  implementation 

Advisors:  Dr.  Mike  Zyda,  Dr.  Bob  McGhee  and  Dr.  Tony  Healey 

/////  •'■//////  /  /  ////////////////////////////////////////////////////////////////// 
#ifndef  VECT0R3D_C 

^define  VECT0R3D_C  //  prevent  errors  in  multiple  tincludes  present 

^include  -ri cs trearr, .  h> 

4ilr.:lude  ''iomanip.h> 

^  I  ti  c  u  o  e  '"'iTt  a  t  n  .  h  > 

class  Vet tor 3D 
{ 

//  member  data  fields 

double  X; 
doutle  y; 
double  z; 

public: 

//  member  constructor  and  destructor  functions 
Vector 3D  (); 

Vector3D  {double  a,  double  b,  double  c) ; 

VectorSD  (const  VectorSDSc); 

--VectorBD  ()  {  /^  null  body  */  } 

//  operators 

VectorSD  &  operator  =  (const  Vector3D&); 

Vector3D  operator  +  (const  Vector3D&); 

Vector3D  operator  -  (const  Vector3D&); 

double  operator  *  (const  Vector3DSc);  //  dot  product 

Vector3D  operator  *  (double) ;  //  scalar  multiplication 

VectorSD  operator  /  (double) ;  //  scalar  division 

VectorSD  operator  (const  Vector3D&);  //  cross  product 

double  Sc  operator  []  (int)  const; 


1 1  inspection  methods 
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double  magnitude  (); 

friend  ostream  &  operator  «  (ostream&  os,  Vector3D&  v) ; 

void  print  { ) ; 

iTiO  d  i  f  y :  n  g  methods 

void  normalize  (}; 

void  normalize  (double) ; 


//////////////////////////////////////////////////////////////////////////////// 

/ /default  constructor 
Vector3D: :Vector3D() 

{ 

X  =  0.0; 
y  =  0.0; 

2  =  0.0; 


//constructor  using  three  doubles 

Vector3D: :Vector3D {double  a,  double  b,  double  c) 

X  =  a  ; 
y  =  b; 


^  constructor  using  another  Vector3D 
VectorSD: :Vector3D (const  Veccor3D&  v) 

i 

X  =  v  .  X ; 
y  =  v.y; 


/ /AssigniTient  operator  -  the  function  must  return  a  reference  to  a  Vector 
//instead  of  a  Vector  for  assignment  to  work  properly 
Vector3Di^  Vector3D :: operator^  (const  Vector3D&  v) 

{ 

X  =  v.x; 
y  =  v.y; 

Z  =  v  .  z  ; 
return  *this; 


//Vector  addition  operator 

Vectcr5D  Vector3D :: operator+ { const  Vector3D&  v) 
( 

Vector3D  sum; 
sum.x  =  v.x  +  x; 
sum.y  =  v.y  +  y; 
sum .2  =  v . z  +  z ; 
return  sum; 

) 

//Vector  substraction 

Vector3D  Vector3D ::  operator- ( const  Vector3D&c  v) 
{ 

Vector3D  diff; 
diff.x  =  X  -  v.x; 
diff.y  =  y  -  v.y; 
diff.z  =  2  -  v.z; 
return  diff; 

} 

//Vector  dot  product 


259 


douiDie  VectorBD operator*  (const  VectorSDs^  v} 

{ 

double  dot; 

-i  o  t  =  (  V  .  X  *  X]  +  ( V  .  y  *  y  )  +  ( v  .  z  *  z  )  ; 

return  dot; 


//scalar  multiplication 

Vector3D  VecCor3D :: operator* (double  n) 

( 

Vector 3D  mult; 
mult.x  =  X  *  n; 
mu 1 1 . y  =  y  *  n  ; 
mult.z  =  z  *  n; 
return  mult; 

) 

//scalar  division  -  it  is  the  user  responsibility  to  make  sure  that  n  is  not 
zero 

VectcrSL;'  Vector3D  ::  operator  /  (double  n} 

t 

V ec  c  o  r  3  D  r e  su  1 1 ; 

result.x  =  X  /  n; 

result.y  =  y  /  n; 

result. z  =  z  /  n; 

return  result; 

} 


void  Vector3D::  print  () 

{ 

cout  <"<■  "["  «  X  <r<  ",  "  <<  y  «  ",  "  «  z  «  '*  ] 


//allows  access  to  the  components  of  the  Vector3D.  it  must  return  a  reference 
//in  order  for  assignment  to  work 
doubled  Vector3D: : operator []( int  n) 
const 
{ 

static  double  result; 

if  (n  ==  1) 

{ 

result  =  X; 

return  result; 

} 

if  (n  ==  2} 

{ 

result  =  y; 

return  result; 

) 
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if  (n  ==  3) 

{ 

result  =  z; 

return  result; 

cout  *'<  “Warning:  Vector3D["  «  n  <<  “]  is  an  invalid  accessor" 
«  “  (only  1..3  allowed),  returning  value  of  0,0“  «  endl; 

static  double  dummy^value  =  0.0; 
return  dumiTiy_value; 


//returns  the  inagnitude  of  the  Vector 
double  Vector3D : :magni tude ( ) 

{ 

return  sqrt ( (x  *  x)  +  (y  *  y)  +  (z  *  z}); 

) 


^  ^'norn'a  1  i zes  the  Vector  to  one 
void  Vector3D :: normalize { ) 

double  m  =  sqrt ( (x  *  x)  +  (y  *  y)  +  (z  *  z)); 
if  (m) 

X  =  x  /  m; 

V  =  Y  ■'  ni : 

2  =  z  /  m  ; 


//normalize  the  Vector  to  d 
void  Vectcr3D :: normalize (double  d) 

double  m  =  sqrt ( (x  *  x)  +  (y  *  y)  +  (z  *  z)); 
if  (mj 

x=d*x/m; 
y  =  d  ^  y  /  m; 

2  =  d  *  z  /  m; 

} 


!  i  I ! !  i  i !  i  n  n  m  I  f  i !  n  I  f  / 1 1  m  1 1 1  m  i !  n  I  m  1 1 1 1  m  m  n ! ! ! !  m !  n !  i !  i  n  m  n !  n 

#endif  //  #ifndef  VECTOR3D_C 
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L. 


Makefile  for  Hydrodynamics  Classes 


n  MaKefile  dynamics  model  for  underwater  virtual  world 
#  14  October  94  Don  Brutzman 


SGIIRIX4FLAGS  =  -02 

#######^ ############################################################### ###### 

#  DIS  includes 

INCF  =  -  I /n/dude/work/brut zman /DIS .mcast /h 

LI&_L'IR  =  -L/n/dude/work/brutzman/DIS  .mcast/src 

LIB  =  /n/dude/work/brut zman/DIS .mcast /src/libdis_client . a 

LIB_ARG  =  -ldis_client  -Impc  -Im 

Hr'R_PATH  =  /n/dude/work/brut  zman/DIS  .mcast/h/ 

HL-RS  =  $  (HDR_PATH)pdu.h  $  (HDR_PATH )  disdef  s  .  h 

########  ################################################# 

CCFLAGS  =  +w  $ (SGIIRIX4FLAGS) 

#INCS  =  -I/n/elsie/work3/macedoni/net/mcast/network/h 

#LIB_DIR  =  -L/n/elsie/work3/macedoni/net/mcast/network/bin 

#LIB  =  /n/elsie/work3/macedoni/net/mcast/network/bin/libdis__client .a 

#LIE_ARG  =  -ldis_client  -Impc  -Im 

#  HDR_  P ATH  =  / n / e 1 s i e / wo  r k  3 / ma  c  e don i / n  e t /me a  s  t / ne  t wo  r k / h / 

#HDRS  S ‘:HDR_PATH)pdu.h  $  ( HDR_PATH) disdef s  . h 

#DET_:iR  =  /n.^elsie/work5/macedoni/net/mcast/network/src/ 

#NET_DIR  =  /n/dude/worK/brutzman/DIS .mcast/src 

#NETOBJS  =  $ (NET^DIR; attach. o  $ (NET_DIR) client_lib. o  \ 

^  S  ::NET_DIR)free.o  S  (NET_DIR) mallocs .  o  $  (NET_DIR)  sends .  o  \ 

$  (NET_DIR)  reevs  .o  $  (NET_DIR)  protocol  .o 


OBJS  =  dynamics.  G  AWsocket.c  Vector3D.o  Quaternion,  o  \ 

Hmat  ri>: .  c  RigidBodi' .  o  DISNetworkedRigidBody  .  oUUVBody  .  o  \ 

SonarModel.o  math_uti 1 i t ies . o 

######^######  ######  ########4^###  ################################ 


dynamics:  $(LIB)  $(HDRS)  $f03JS) 

@echc  " Linking ..." 

CC  $ (SGIIRIX4FLAGS)  $(LIE_DIR)  ${OBJS)  $(NETOBJS)  $ (INCS)  $(LIE_ARG)  \ 
-o  dynamics 

#  Makefile  req^uirement  is  that  no  libraries  are  linked  to  the  objects 


Vector3D.o:  Vector3D.C 

CC  $( SGI IRIX4 FLAGS)  $  (LIBJIR)  $(NETOBJS)  ${INCS)  $(LIB_ARG)\ 
-c  VectorSD.C  -o  Vector3D.o 

Quaternion . o :  Quaternion . C  Vector 3D . C 

CC  $ (SGI IRIX4 FLAGS)  $ {LIB_DIR)  ${NETOBJS)  ${INCS)  $(LIB_ARG)\ 
-c  Quaternion. C  -o  Quaternion. o 

Hmatrix.o:  Hmatrix.C  VectorSD.C  Quaternion. C 

CC  $(SGIIRIX4FLAGS)  ${LIB_DIR)  $(NETOBJS)  $(INCS)  $(LIB_ARG)\ 
-c  Hmatrix.C  -o  Hmatrix.o 
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RiqidBody.o:  RigidBody.C  Hmatrix.C 

CC  $ {SGIIRIX4FLAGS}  $(LIB_DIR)  $(NETOBJS)  ${INCS)  ${LIB_ARG}\ 

-c  RigidBody.C  -o  RigidBody.o 

AW^^ocket.o:  AUVsocket.C  AUVglobals.H  UUVmodel.H  Quaternion. C  ${HDRS) 

CC  $ (SGIIRIX4FLAGS)  ${LIB_DIR)  $(NETOBJS)  ${INCS)  $(LIB_ARG)\ 

-c  AUVsocket.C  -o  AUVsocket.o 

DISNetworkedRiqidBody .0:  DISNetworkedRigidBody . C  AUVglobals.H  UUVmodel.HX 
RigidBody.C  $ (HDRS) 

CC  $ (SGIIRIX4FLAGS)  ${L1B_DIR)  $(NETOBJS)  $(INCS)  $(LIB_ARG)\ 

-c  DISNetworkedRigidBody .C  -o  DISNetworkedRigidBody . o 

SonarModel . o :  SonarModel.C 

CC  $ (SGIIRIX4FLAGS)  ${LIB_DIR)  $(NETOBJS)  $(INCS)  $(LIB_ARG)\ 

-c  SonarModel.C  -o  SonarModel. o 

UlTVBody.o:  UUVBody.C  AWg  1  oba  1  s  .  H  UUVmodel.H  SonarModel.C  AUVsocket.C\ 
DISNetworkedRigidBody . C 

CC  $ (SGIIRIX4FLAGS)  $ {LIB^DIR)  $(NETOBJS)  $(INCS)  $(LIB_ARG)\ 

-c  UUVBody . C  -o  UUVBody . o 

dv  c  aiTi  i .  c :  dynamics.  C  AUVglobals.H  UUVmodel.H  UUVBody.C  $(HDRS) 

"'"cC  S  {SGiIRIX4FLAGS)  ${LIB_DIR}  $(NETOBJS)  $(INCS)  ${LIB_ARG)\ 

-c  dynamics. C  -o  dynamics. o 


m.a  t  1 1 1  i  1 1  es  .  c  :  mat  h_u  tiiities.C 

CC  $ (SGIIRIX4FLAGS)  $(LIB_DIR)  $ (NETOBJS)  ${INCS)  $(LIB_ARG)\ 
-c  math^ut i 1 1 t ies . C  -o  math_utilities . o 

AUValobals.H  UUVmodel.H  $(HDRS) 

#  CC  -C  $(CCFLAGS)  $(INCS)  $*.C 

#  utilities: 
delete : 

rm  -f  core  *.o  *.a  a.  out  d^'^namics 

clean : 

rrri  -f  core  *.o  *.a  a.  out 
all:  delete  dynamics 
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VI.  SONAR  MODEL 


A.  SonarModeLC  Geometric  Sonar  Model 


/// ///////////////////////////////////////////////////////////////////////////// 
P r  og  r arr, :  S oi'i a rMode  1 .  C 

Descript  I  on :  Underwater  vehicle  geometric  sonar  model 

Revised:  21  October  94 


Comp il at  ion : 

Advisors : 
Author : 


I  rix 

ANSI  C-r-^ 

irix>  make  dynamics 

irix>  CC  SonarModel.C  -Im  -c  -g  +w 

-c  ==  Produce  binaries  only,  suppressing  the  link  phase. 
+w  ==  Warn  about  all  questionable  constructs. 

Dr.  Mike  Zyda ,  Dr.  Bob  McGhee  and  Dr.  Tony  Healey 

Don  Brutzman  brutzman@cs.nps.navy.mil 

Code  OR/Er 

Naval  Postgraduate  School  (408)  656-2149  work 

Monterey  CA  93943-500C  (408)  656-2595  fax 


Only  pool  geometric  model  included 


Future  wotk: 


Comments  and  suggestions  are  welcome! 


/////  ////// //////////////////////////////////////////////////////////////////// / 
# 1 f  ndef  SGNARMODEL_C 

#deflne  SGKARMODEL_C  //  prevent  errors  if  multiple  #includes  present 
#iiiclude  "AUVcocket  . C " 

int  precede  (double  anglel,  double  angle2j; 

int  precede_radians  (double  anglel,  double  angle2); 

void  test_tank_sonar_model  { ) ;  //  parallelize  &  generalize  this  sonar  model 

^  ,  ,  ,  //  for  all  objects  in  vw,  using  Ziomek's  RRA 

double  radian_normalize  (double  rads); 
double  radian^normalizeZ  (double  rads); 

///////////////////// ///////////////////////////////////////////////////////y/// 

/  *  ^  ^ 
/*  excerpt  from  geometric  reasoning  circle  world  circle. c  */ 
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/*  Boolean  function  for  angle  precedence  */ 


int  precede  (double  anglel,  double  angle2) 

/*  angles  are  any  real  angles  in  degrees  */ 
/*  return  TRUE  if  anglel  precedes  angle2,*/ 
/*  return  FALSE  otherwise  */ 

{ 

/*  note  that  the  input  angles  are  individually  normalized  to  ensure  validity  */ 

if  {normali2e2  (normaiize2  (angle2)  -  normali2e2  (anglel))  >  0.0) 
return  TRUE; 

else  /*  reference:  equation  (7)  class  notes  */ 

return  FALSE; 

} 

/* - */ 

double  radian_normalize  (double  rads)  /*  radians  input*/ 

{ 

double  result  =  rads; 

while  ''result  <  0.0)  result  +=  2.0  »  PI; 

while  'result  >-  2.0  *  PI)  result  -=  2.0  ^  PI; 

return  result; 

- 


douhle  racil ari_normai i  ze2  (double  rads)  /*  radians  input*/ 

J 

dcutde  result  =  rads; 

while  (result  <=  -  PI)  result  +=  2.0  *  PI; 
while  (result  >  PI)  result  -=  2.0  *  PI; 

return  result; 


- */ 

/*  excerpt  from  geometric  reasoning  circle  world  circle. c  */ 

/*  Boolean  function  for  angle  precedence  */ 

int  precede_radians  (double  anglel,  double  angle2) 

/*  angles  are  any  real  angles  in  degrees  */ 
/*  return  TRUE  if  anglel  precedes  angle2,*/ 
/*  return  FALSE  otherwise  */ 

/*  note  that  the  input  angles  are  individually  normalized  to  ensure  validity  */ 

if  (radian_normali2e2  (radian_normali ze2  {angle2)  - 

radian_normalize2  (anglel))  >  0.0) 


return  TRUE; 

else  /*  reference:  equation  (7)  class  notes  */ 

return  FALSE; 

} 

/» - */ 
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////////////////////////////////7//7/ /////////////////////////////// //////////// 


void  i:e:?r_tank_sonar_Tncdel  ()  //  parallelize  &  generalize  this  sonar  model 

( 

dcjtie  5A,  SB,  SC,  SD;  //  angles  from  sonar  to  corners 

//  Test  tank  coordinates:  A  (-10,  10)  +--+  (10,  10)  B  X-axis 

//  !  I 

//  ill 

//  D  (-10,  -10)  +--+  (10,  -10)  C  +->Y-axis 


double  return_x,  return_y,  distance;  //  intersect  coordinates  &  distance 
//  Air^_ST1000_bearing  and  AUV_ST72 5_bearing  determined  by  vehicle  control 


// 

ST 

1000 

conical 

pencil 

beam 

bearing 

A17 

'’_5T  1 0  0  0_r  ange  =  0.0; 

// 

ST^ 

1000 

conical 

pencil 

beam 

range 

AU\ 

^_ST1000_st renqth=  0.0; 

// 

ST_ 

JOOC 

conical 

pencil 

beam 

strength 

// 

ST„ 

725 

1  X  24 

sector 

beam 

bearing 

T  T' 

n'.'  . 

^_ST7 2  Strange  =  0.0; 

// 

ST 

725 

1  X  24 

sector 

beam 

range 

AU^v 

^_ST72 5_strength  =  0.0; 

// 

ST^ 

725 

1  X  24 

sector 

beam 

strength 

double  sonar_x  =  Air/_x  +  cos  (AUV_psi)  *  AW_ST100  0_x_of f set ; 
double  sonar_y'  =  AUV^^  +  sin  (AUV__psi)  *  AUV_ST1000_x_of f set ; 


//  reiTicve  these  lines  when  auv  vw  viewer  includes  sonar  offset 
s  o  n  a  r _x  =  AUV_x ; 

senary'  =  Ab7^^/; 

double  sonar_psi  =  AW^psi  +  radians  (AUV_ST1000_bearing )  ; 

10.0)  && 

10.0}  SeSe 

41.0) ) 

//  return  expected 
//  return  expected 

//  note  that  a  reversed  x,y  calling  sequence  is  necessary 
//  in  order  to  get  correct  quadrant  alignment 
SA  =  atanl  ((-IC.O;  -  sonar_v',  (  10.0)  -  sonar_x)  ; 

SB  atanl  ((  10.0)  -  sonar_y,  (  10.0]  -  sonar_x)  ; 

SC  =  atan2  ((  10.0}  -  sonar_y,  (-10.0)  -  sonar_x) ; 

SL  =  atan2  ((-10.0)  -  sonar^^  (-10.0)  -  sonar_x) ; 

if  (TRACE) 
i 

cout  «  "AUV_x  =  *'  «  AUV_x 

«  " ,  AUV_y  =  ”  «  AUV_y 

«  ",  AIJV_psi  =  "  «  degrees  {AUV_psi)  «  endl ; 
cout  «  "sonar_x  =  "  «  sonar_x 

«  " ,  sonar_y  =  "  «  sonar_y 

«  ",  sonar_psi  =  "  «  degrees  (sonar_psi)  «  endl ; 
cout  «  "SA  =  "  «  degrees  (SA) 

«  ",  SB  =  "  «  degrees  (SB) 

«  " ,  SC  =  "  «  degrees  (SC) 

«  ",  SD  =  "  «  degrees  (SD) 

«  endl ; 

cout  «  "precede_radians  (SA,  AUV__psi)  =  " 

«  precede_radians  (SA,  AUV_psi)  «  endl; 

} 

if  (  precede_radians  (SA,  AUV_psi) 

precede^radians  (AUV_psi,  SB)) 


•f  iAW_x  <=  10.0)  &&  (AOT_x  >=  - 
^A17^  <=  10.0)  &&  (AUV^  >=  - 
(AW_z  <=  50.0)  aU  (AUV_2  >= 

A7i_STi 0  0  0_s  t  reng  t h  =  10.0; 
AW_ST725_strength  =  10.0; 
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(10.0 


sonar_x) ; 


if  (TRACE)  cout  «  "SECTOR  1“  <<  endl ; 
return^x  =10.0; 

rerurn_:y  =  sonar_y  +  sin  (sonar_psi)  * 

else  if  {  precede_radians  (SB,  sonar_^si) 

&&  precede__radians  (sonar_psi/  SC}) 

^  if  (TRACE)  cout  «  "SECTOR  II*'  «  endl; 

return_x  =  sonar_x  -  sin  (sonar_psi  -  PI/2.0)  *  (10.0  -  sonar_y) ; 
return_y  =  10.0; 

else  if  (  precede_radians  (SC,  sonar_psi) 
precede_radians  (sonar_psi,  SD) ) 

^  if  (TRACE)  cout  «  "SECTOR  111“  <<  endl  ; 
return_x  =  -  10.0; 

return^  =  sonar_y  -  sin  (sonarjsi  -  PI)  *  (sonar_x  +  10.0); 

else  if  (  precede_radians  (SD,  sonar_psi) 
precede^radians  (sonar_psi;  SA)  ) 

if  (TRACE)  cout  «  "SECTOR  IV"  «  endl; 

return_x  =  sonar_x  +  sin  (sonar_psi  +  PI/2.0)  *  (sonar_y  +  10.0); 
re  tu  rn_y  =  -  10.0; 

else  cout  "Computational  sonar  error,  no  sector  was  determined." 
endl  ; 


distance  =  sqi 


(  (return_x  -  sonar_x)  *  (return_x  -  sonar_x) ^ 

+  (return^y  -  sonar_y)  *  (return_y  -  sonar_y)}; 


AIT;_ST1  000_range  =  distance; 
AC"/_ST7  2S_range  =  distance; 


"return_x 


«  " ,  return_:/  = 


«  return_x 
«  return_j/  «  endl; 


cou’-  "AUV_ST1000__range  =  "  «  AUV_ST1000_range 

^  AUV_ST7 2 Strange  =  "  «  AUV_ST7 2 Strange  «  endl ; 


else  return;  //  outside  of  test  tank,  default  return  is  0 
//  end  test_tank_sonar_model 

/////./////// ////////////////////////////////////////////////////////// /////// 
dif  i:  SONARMODEL_C 
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VII.  NETWORKING 


A.  Introduction 

The  principal  networking  code  used  in  the  underwater  virtual  world  has 
already  been  presented  in  previous  listings.  Source  programs  execution.c  and 
AUV socket. c  implement  socket  communications  between  robot  and  virtual  world,  wMle 
DlSNetworkedRigidBody.C  and  viewer. C  communicate  via  DIS  PDUs  over  the  MBone. 
These  communications  links  were  not  created  from  scratch.  The  following  test 
programs  were  used  in  program  development  and  are  included  as  valid  standalone 
applications  that  can  be  used  to  test  network  software,  independently  of  the  robot  or 
computer  graphics  applications. 

os9sender.c  and  os9server.c  are  a  client/server  pair  used  to  open  test  and  close 
a  point-to-point  socket  What  is  unusual  about  these  programs  is  that  they  were 
developed  to  operate  interchangeably  on  either  Unix  systems  using  Berkeley  Software 
Distribution  (BSD)  4.3  network  routines,  or  on  OS-9  processors  using 
Microware -provided  network  routines  (Microware  91a,  91b).  Network  code  is  difficult 
to  debug  so  extensive  trace  options  are  provided  to  assist  in  diagnosing  difficulties. 

Prior  to  the  current  multicast  version  (2.0.3)  of  the  DIS  library  distribution,  aU 
DIS  interfaces  used  broadcast  or  unicast  transport  protocols.  This  meant  that  all 
network  interactions  between  DIS  applications  were  restricted  to  a  single  local  area 
network  (LAN).  Such  a  limitation  is  unacceptable  if  an  underwater  virtual  world  is  to 
be  capable  of  communicating  globally  across  the  Internet,  disbridge.c  was  written  to 
create  a  socket  bridge  between  LANs  which  reflected  DIS  PDUs  identically  in  either 
direction.  Multiple  disbridge  programs  could  connect  arbitrary  numbers  of  LANs  with 
only  a  single  disbridge  needed  for  each  new  network  addition.  Although  such  an 
approach  is  inconvenient  and  a  somewhat  inefficient  use  of  bandwidth,  it  can  extend 
the  broadcast/unicast  DIS  approach  to  large  numbers  of  networks. 

disbridge  has  been  tested  successfully  with  earlier  versions  of  the  underwater 
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virtual  world,  and  also  with  the  NPS  Platform  Foundation  (Bailey  94).  Fortunately 
disbridge  is  no  longer  needed  since  the  newer  multicast  DIS  version  2.0.3  can  take 
advantage  of  the  MBone  to  attain  Internet-wide  connectivity  (Zeswitz  93). 

Nevertheless  only  about  a  thousand  subnets  are  currently  connected  to  the  MBone,  and 
conceivably  disbridge  might  someday  be  useful  to  connect  networks  independently  of 
the  MBone.  Recommended  future  work  includes  upgrading  disbridge  to  be  compatible 
with  future  versions  of  the  DIS  library  in  unicast,  broadcast  or  multicast  modes. 
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B.  os9sender.c  Unix  to  OS-9  Socket  Communications  Client 


Prograiri:  os  9  sender,  c 

Descript  ion :  socket  test  program  to  pass  input  strings  in  2  directions, 

especially  socket  from  OS-9  auvsiml  to  Irix  fletch,cs 

Revised:  31  MAY  94 

Compilation:  unix>  cc  os9sender.c  -o  os9sender 

os-9>  make  os9sender 

Execution:  hostl>  os9server 

host2v  os9sender  -r  hostl 

Example:  fletch>  os9server 

auvsiml>  os9sender  -r  fletch 


Execution  options: 

-r  -h  -t  (first  letters  are  sufficient,  capital  letters  are  OK  too) 

-remote  hostname  hostname . remote . net , address  for  client  to  connect  to  server 
no  default  is  present 

-help  display  calling  syntax 

-trace  turn  on  TRACE  mode 


References:  (1)  (Gespac-provided)  EVIRz^  EVLAN-11  Ethernet  Data  Link 

Controller  for  the  G64/G96  Bus/EVTCP  Internet  Package 
technical  manuals 

(2)  Internetworking  with  TCP/IP  Volume  I:  Principles, 
Protocols  and  Architectures,  Douglas  E,  Comer, 

Prentice  Hall,  Englewood  Cliffs  NJ,  1991 

(3)  Internetworking  with  TCP/IP  Volume  II:  Design, 
Implementation  and  Internals,  Douglas  E,  Comer  and 

David  L.  Stevens,  Prentice  Hall,  Englewood  Cliffs  NJ,  1991 

(4)  IRIX  Network  PrograrruTiing  Guide,  Silicon  Graphics  Inc. 

(5;  An  Advanced  4.3BSD  Interprocess  Communication  Tutorial, 

Samuel  J.  Leffler,  Robert  S.  Fabry,  William  N.  Joy, 

Phil  Lapsley,  Steve  Miller  and  Chris  Torek,  undated 
(6)  Real-Time  Programming  Tutorial,  Bill  Mannel ,  SGI  Expo, 
Silicon  Graphics  Inc.,  23  May  93 
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brut2man@cs . nps .navy .mil 

(408)  656-2149  work 
(408)  656-2595  fax 

Szazus:  Tested  satisfactory  I rix<==>Irix,  OS-9<==>OS-9 ,  Irix<==>OS-9 

Future  work;  Consider  implementation  as  an  inetd  'superserver'  daemon. 

Ensure  compatibility  with  DIS  v2 . 0  protocols  when  available. 

add  #elif  (sun)  defines 

Comments,  suggestions  and  corrections  are  welcome! 


Author;  Don  Brutzman 

Code  OR/Br 

Naval  Postgraduate  School 
Monterey  CA  93943-5000 


^^^i,^i,ir-g-»;i^**irt;ic-r*ic**t:******-k**:************tt*if*******'^***********************'*^**/ 

Irix  defines  in  /usr/include  */ 
defined isql) 


^?include  <stdio.h> 

# include  <sys / types . h> 

# include  <sys /socket . h> 

# include  <netinet /in , h> 

1 1'!  c  1  ud e  n e  t  dl:- .  h > 

^include  <signal.h> 

# include  <string.h> 

OS-9  subdirectories  /DEFS,  /DEFS/INET  */ 
^  e  ^  se 

i?:nclude  ‘'3tdio.h> 

# include  < types. h> 

^include  ^inet /socket . h> 

<iinclude  -'inet/in.h> 

4»include  <inet /netdb . h> 

^include  <signal.h> 
include  <errnc.h> 


#endi  f 

4iinclude  <time.h> 


/*  One  stream  socket  is  used  with  adequate  throughput  */ 

/»  (although  two  could  work,  no  performance  improvement  is  expected)  */ 


Be  careful  that  you  reserve  these  port  numbers  to  prevent  collisions 
/*  froiT'  other  processes  requesting  ports  on  your  system: 


*/ 

*/ 


# define  DISBRIDGE_TCP_PORT 


#define  AUVSIM1_TCP_PORT_0 
#define  AUVSIM1_TCP_P0RT_1 
# define  AIA/£IM1_TCP_P0RT_2 
#define  AUVSIM1_TCP_P0RT_3 
#define  AUVSIM1_TCP_P0RT_4 
#define  AUVSIM1_TCP_P0RT_5 
^define  AUVSIMl_TCP_PORT_6 
# define  AUVSIMl_TCP_PORT_7 


2056  /*  disbridge  program,  server  &  client  */ 

/*  NPS  Autonomous  Underwater  Vehicle  (AUV)*/ 
/*  Underwater  Virtual  World  (UVW)  */ 


3210 

/* 

os9sender  <==>  os9server  test  programs 

*/ 

3211 

/* 

auv  execution  level  <==>  virtual  world 

*/ 

3212 

/* 

port  for  future  use 

V 

3213 

/* 

port  for  future  use 

V 

3214 

/* 

port  for  future  use 

*/ 

3215 

/* 

port  for  future  use 

V 

3216 

/* 

port  for  future  use 

*/ 

3217 

/* 

port  for  future  use 
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# define  Al.T\'£IMl  TCP  PORT  8 

3218 

/* 

port 

for  future  use 

*/ 

#d6fine  AUVEIM1_TCF_P0RT_9 

3219 

/* 

port 

for  future  use 

*/ 

*  define  TRUE  1 

#  define  FALSE  0 

Sdefine  £OCKET_QUEUE_SI ZE  5 

/*  max  allowed 

by  TCP/IP 

*/ 

I"*  f unct  ^  on  prototypss  *********************************************************** 


/ 


/»  Note  that  function  prototype  parameters  may  need  to  be  deleted  if  the 

C  compiler  used  is  not  an  ANSI  compiler  (e.g.  OS-9  KScR  C  compiler)  */ 


void  shu tdown_os9sender  (); 


/*  global  variable  definitions  ********** 


static  int 


TRACE  =  0; 


»-*********if********* 

/■^  1  =  trace  on. 


****»**★★★*<■****  j 

0  =  trace  off  */ 


static  int 


socket_des crip tor, 

socket_accepted, 

socket_stream; 


Stacie  int 
static  int 


int 


static  int 


socket_length  =  255; 

by tes_received,  bytes_read,  bytes_written, 
bytes_left,  bytes_sent; 

shutdown_signal_received  =  FALSE; 

i  =  0, 

print_help  =  FALSE; 


^•^■^★•JI-^X'JryxXT**********************'************'*******-#:******'**********-***'*’*****  j 


main  (arge,  argv) 

int  arge;  char  *'*argv; 

J 

char 

FILE 

struct  sockaddr_in 
register  struct  hostent 
char 

static  char 
static  char 


/*  command  line  arguments  */ 

command_sent  [81], 

command_received  [81], 
remote_host_name  [60]; 

*netstat_f ileptr; 

server_address ; 

*  s  e  r ve  r_en  t i ty ; 

test_buffer  [81]; 

*ptr  ; 

*remote_buf  f  er  ; 


y********it*****ir**icirir*-icie***-******if*****-icicitititic*-tie-*icit*iticie****ic*itir*iticit***irir*itit****  j 

/*  Begin  execution,  parse  command  line  */ 

for  (i  =  1;  i  <  arge;  i++) 


if  {{argv[i][0]  !=  '-')  M  print_help) 
{ 
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print_help  = 
break ; 

TRUE 

switch  largviij 

[1]  ) 

/* 

case  ' h ' : 

/* 

help 

case  ' H ' : 

/* 

Help 

case  ' ? ' : 

/* 

Help 

print_help  =  TRUE; 
break ; 

case  'r' :  /*  remote  hostname  tells  client  who  to  connect  to  */ 

case  'R' : 

/*  get  remotehost  name  */ 

if  {i+1  <  argc)  strcpy  (remote_host_name,  argv[++i]); 
else  print_help  =  TRUE; 

break ; 

case  't':  /*  TRACE  feature 

case  'T' : 

TRACE  =  In¬ 
break; 

default:  /*  no  command  line  entry  parameter  present  */ 

prlnt_help  =  TRUE; 

•  /*  end  switch  based  on  current  command  line  parameter  */ 
r  /*  end  for  */ 

If  large  ==  1  ! I  print^help)  /*  print  help  string  ***.****************** 

orintf  (“Usage;  os9sender  -remote  hostname  [-help]  [ -trace] \n" } ; 


if  (TRACED  print f  f  “[ os9sender  TRACE  on]\n‘'); 
ff^usn  (stdinj ; 

Comr^a nd  line  parameter  parse  complete 
Initialize  communications  blocks 


r**^»*'******»***'<r***»**^ 


/*  set  socket_length 
socket_length  =  81; 


/*  maximum  allowed  packet  size  */ 


/^»»***»***»»»*»*ic*****»**********»*************»»****’^***********************’^/ 

/*  Initialize  both  client  &  server  *»*^*************-*****’^***************^*** V 

/*  signal  handlers  for  termination  to  override  net_open  ()  and  net_close  ()*/ 
/*  signal  handlers.  Otherwise  you  are  unable  to  ^C  kill  this  program.  */ 


#if  defined(sgi) 

signal  {SIGHUP,  shutdown_os9sender ) ; 
signal  (SIGINT,  shutdown_os9sender) ; 
signal  (SIGKILL,  shutdown_os9sender ) ; 
signal  (SIGPIPE,  shutdown_os9sender) ; 
signal  {SIGTERM,  shutdown_os9sender ) ; 
#endif 


/*  hangup  */ 
/*  interrupt  character  */ 
/*  kill  signal  from  Unix  */ 
/*  broken  pipe  from  other  host  */ 
/*  software  termination  */ 


/***************»***»*****^*****************'***********************************/ 
/*  Initialize  sender  ....*...♦•*.♦•»*»♦***♦•*******•*•**•****»*•**•»*•••»******/ 

/*  start  by  finding  default/desired  remote  host  to  connect  to  */ 
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server_enti ty  =  gethostbynaine  ( remote^host^name)  ; 
if  ( serve r_enti ty  =-  NULL) 

{ 

printf ( “ os9sender  -remote  host  (\"%s\“)  not  found\n", 
remote_host_namG} ; 
exit  { -1 ) ; 

} 

else  if  (TRACE) 

printf  {" os9sender  remote  host  (\"%s\'')  located\n",  remote__host_name)  ; 

/*  Client  opens  server  port  ^**************** ************★****♦**********/ 

/*  Fill  in  structure  ' server_address '  with  the  address  of  the  */ 

/*  remote  host  (i.e.  SERVER)  that  we  want  to  connect  with:  */ 

#if  def ined ( sqi ) 

bzero  ((char  *:  &server_address ,  sizeof  ( server_addr€ss ) ) ; 

^endif 

server_addr€ss . sin_f amily  =  AF_INET;  /*  Internet  protocol  family  */ 

/■*■  copy  server  IP  address  into  sockaddr_in  struct  server^address  */ 

#if  definedisgi) 

bcopy  (server_entity->h_addr ,  & {server_address . sin_addr . s_addr) , 
server_enti Cy->h_length) ; 

T?  e  1  se 

strncpy  iSc  { server_address  .  sin_addr  .  s_addr)  ,  server_entity->h_addr , 
server_ent i ty->h_length)  ; 

^  ei'iGi  I 

/*  make  sure  port  is  in  network  byte  order  */ 

server_address . sin_port  =  htons  (AUVSIM1_TCP_PORT_0 ) ; 

/*  Open  TCP  (Internet  stream)  socket  */ 

if  (  (socket_descriptor  =  socket  (AF_INET,  SOCK_STREAM,  0))  <  0  ) 

{ 

printf  r'os9sender  client  can't  open  server  stream  socket"); 
exit  {-1 ) ; 

) 

else  if  (TRACE) 

printf  ("os9sender  client  opened  server  socket  successful ly\n" ) ; 

/ Connect  to  the  server.  Process  will  block/sleep  until  connection  is 

is  established.  Timeout  will  return  an  error.  */ 

if  (connect  (  socket^descriptor^ 

(struct  sockaddr  *)  &server_address, 

sizeof  {server_address) )  <  0) 

printf  ("os9sender  client  can't  connect  to  server  socketXn"); 
exi t  ( - i ) ; 

else  if  (TRACE) 

printf (" os9sender  client  connected  to  server  socket  successfully \n" ) ; 
)  /*  end  initialization  */ 

/****************.**★★****»★**★***★***★****•**»****★★■*-********★******************  y 

/*  loop  transferring  telemetry  until  shutdown_signal_received;  */ 

y**********************************************************************^*******y 

/*  Two-way  reflector:  listen  to  local  program  and  relay  to  remote  host, 

listen  to  remote  host  and  relay  to  local  program  */ 

socket_stream  =  socket_descriptor;  /*  client  */ 

if  (TRACE) 

{ 

printf  ("os9send€r  CLIENT:  socket_descriptor  =  %d,\n'‘,  socket_descriptor)  ; 
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print f  (" 
printf  {" 


socket_accepted  =  %d,\n",  socket_accepted) ; 

socket__stream  =  %d\n“,  socket_stream)  ; 


/•*■  teL-t  handshaker 


write  (socket_st rearr.,  “SUCCESS  #2:  os9sender  connected  to  os9server!",  46); 
read  (socket_st ream,  test_buf fer,  46) ; 
r  est_b'af  f  er  [47]  =  '\n'; 

printf  ("test  handshake  between  hosts:  \n%s\n“,  test_buf fer) ; 


/★**»***★*★**★**★****★************»*** 


★  ★*•*■**•**★****•*:*★★*★★**•**********♦*****■**/ 


/*  initialize  boolean  for  main  loop  control  */ 
shutdown's ignal_received  =  FALSE; 

while  ( shu tdown_signal_received  ==  FALSE)  /*  loop  until  shutdown..  TRUE  */ 

^*ic**ir*******-X*ic-k*-k****-k*-k****-k-k-K**iric-k******************tc**-k****-k**********'^/ 

/*  Sender  block 

,  ^^^^^^i,,,irir-^^lr*ir-^-^**-^**icic***fc-)^-kir**’t(*-k**-k**-k************-k**iC********-k****-*C***^ 

/*  read  from  local  stdin,  relay  to  remote  host  */ 


s  ( ccmmand_sent I ; 


by tes_received  =  strlen  (command_sent ) ; 

if  (TRACE;  printf  ("[os9sender  command_sent : %s ] \n“ ,  command_sent ) ; 

if  'by tes_recelved  0)  /*  read  failure  */ 

printf  ■' "osRsender  gets  ()  from  keyboard  unsuccessful \n'' )  ; 
shutdown^cs 9 sender ( )  ; 


if  ■' by tes_received  >  socket_length ) 

orintf  ("osRsender  s end_t el erne try_t observer  error:  "); 
printf  .  "bytes_recGived  toe  big  for  packet  socket_length\n'' )  ; 
printf  ("  •' )  ; 

printf  ( " [bytes_rec€ived=%d]  >  [ socket_length=%d] ;  ", 

by tes_received,  socket_length) ; 

printf  {“string  truncatedXn" ) ; 

} 


by tes_lef t 
by tes_written 
ptr 


socket_length; 

0; 

command_sent ; 


while  ({bytes_left  >  0)  St&c  (by tes_written  >=  0))  /*  write  loop  **★*★****♦*/ 

{ 

bytes_sent  =  write  (socket_stream,  ptr,  bytes_lGft); 


if 

else  if 
{ 


(bytes_sent  < 
(bytes_sent  > 

by tes_lef t 
by tes_wri tten 
ptr 


0)  by tes_written 
0) 

-=  bytes_sent; 
+=  bytes_sent; 
+=  bytes_sent; 


bytes_sent ; 


} 

if  (TRACE) 

printf ( "os9sender  send_telemetry_to_server  loop  bytes  sent  =  %d\n“, 
by tes_sent ) ; 
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if  (by tes_written  <  0} 

{ 

printf  ('■os9sendGr  s  en  d_t  el  erne  try_t  observer  ()  send  failed,  "); 
printf  ("%d  bytes_wri t cenXn" ,  bytes_written} ; 

) 

else  if  (TRACE) 

printf  ("os9sender  send_telemetry_to_servGr- total  bytes  sent  =  %d\n*‘, 
bytes_written) ; 

I*  Check  termination  */ 

/:rTr-jr-r*******»Tr****»*»******-*»*********-»****************'*******:A:*'************/ 

if  (strncmp  ( comma nd_s en t ,  “shutdown",  8)  ==  0) 
shutdown_signal_received  =  TRUE; 

if  { shutdown_signal_received  ==  TRUE) 

shut down_os9 sender  ( ) ; 
break ; 

^/*»*-r**-*-x**»*»**-»******x**********»**»**********»**-*r********-*******-*********/ 

/*  Receiver  block  */ 

/*  listen  to  remote  host,  relay  to  local  network /program  */ 

bytes_lefc  =  socket_length; 

by res_received  =  0; 

ptr  =  comm.and_received; 

while  { {bytes_left  >  0)  (by tes_received  >=  0))  /*  read  loop  ***********»/ 

( 

byces_read  =  read  ( socket_streami,  ptr,  bytes_left); 

if  (bytGS_read  <  0)  bytes_received  =  bytes_read; 

else  if  (bytes_read  >  0) 

{ 

byres_leit  -=  bytes_read; 

by tes_recei ved  +=  bytes_read; 
per  +=  bytes_read; 

} 

if  (TPUiCE) 

printf  ("os9sender  receiver  block  loop  bytes_read  =  %d\n“, 
byte spread) ; 

/*  if  nothing  is  waiting  to  be  read,  break  out  of  read  loop  */ 

if  ((bytes_read  ==  0)  (by tes_received  ==  0))  break; 

) 

if  (byt€S_received  <  0)  /*  failure  */ 

{ 

if  (TRACE) 

{ 

printf  ("os9sender  receiver  block  read  failed,  "); 
printf  ( “bytes^received  =  %d\n'’,  bytes__received)  ; 

} 

1 

else  if  (bytes_received  ==  0)  /*  no  transfer  */ 

{ 

if  (TRACE) 

printf  ( ''os9sender  get_PDU_f  rom_other_host  read  received  0  bytesXn*); 

} 

else  if  (bytes_rGceived  >  0)  /*  success  */ 

printf  (“%s\n",  comma nd_recei ved ) ; 
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/**tcyir***ir-tc-k**irir**ic***ifkie******'*-k**-k**iriric*-k*ieicic***-k**ieie-k*ir******************^ 

/*  Check  termination  */ 

^■)ctr-K**-k-kte**irir**ir**-ki(***ie*-k-k**-k*-K**icir-k-k*it*’k-k**ic*-k*-k*ieiric*-k***ic*'k**************^ 

if  .-.r  rnciTtp  {command^recei  ved,  "shutdown'*,  6)  ==  0) 
shutdown_slgnal_receiveG  =  TRUE; 


if  {shutdown_signal_received) 
I 

shutdown_os9sender  ( ) ; 


break ; 

) 

)  /*  end  of  while  i 

( shutdown= 

=  FALSE)  indefinite  loop  "^7 

shutdown^os 9 sender 

0  ; 

/*  ensure  shutdown_os9sender  is  always  reached  */ 

printf  ("osPsender 

exit  Xn") 

; 

exit  (0);  /*  os9sender  complete  */ 

1 


^/★»»**»**»****-»-******-»-*x*»*****»*****»***Tr**»************C***»*****^*-**********/ 

/***»»*»'Tr**»*»»-*-»-*»-***if**»-***Tr******»****-rT»:***^:**^******^******^**^*-*±*<r***-***»/ 

•'  end  of  main  program  */ 

/■j*-*Tr-r»x'»r'»-»»:^*»»**-»-**'jr»***»**********-********-**'*-**********-************'**»***»**'*^ 

.  x*xiii  x»x»x»x**»xx*xx*»x*xxx»x**xx»»*»******************-************************  ! 

/’  Shutdown  block  */ 

void  3hutGown_os9sender  () 

; 

if  (TRACEi  printf  ("os9sender  shutdown  in  progress  ...Xn"); 

shutdown_signai_received  =  TRUE;  /*■  in  case  entry  was  from  signal  handler  */ 

iNo  need  to  send  a  message  to  other  side  that  bridge  is  going  down,  */ 

/*  since  SIGPIPE  signal  trigger  may  shutdown_os9sender { )  on  other  side?  */ 

if  (close  ( socket_stream]  ==  -1} 

printf  ("osPsender  close  ( socket_streami)  f ailedXn" )  ; 

if  (TRACE)  printf  ("osPsender  shutdown_os9sender  ()  complete \n ") ; 
return ; 

}  /*  end  shutdown_os9sender  {}  */ 

/********************★★***★*********•*********★******★**★**■*******★•*★*★***★***★*/ 

/★♦★xxxxxx-*-************************************************^^************^*****/ 
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c. 


os9server.c  Unix  to  OS-9  Socket  Communications  Server 


r*******-*.****^***»*******;jt******************^ 


Program: 

os9server . c 

Description: 

socket  test  program  to  pass  input  strings  in  2 

di rections, 

especially  socket  from.  OS-9  auvsiml  to  Irix 

auvsim4 

Rev: sed : 

20  JAN  94 

Comp i 1  a  t i on : 

unix>  cc  os9server.c  -o  os9server 
os-9>  make  os9server 

Execution : 

hostl>  os9server 

host2>  os9sender  -r  hostl 

Example: 

fletch>  os9server 
auvsiml>  os9sGnder  -r  fletch 

Execucior,  opt  Ion 5: 

-h  -t 

(first  letters  are  sufficient,  capital  letters 

are  OK  too) 

-help 

display  calling  syntax 

-  trace 

turn  on  TRACE  m.ode 

References:  '1;  (Gespac-provided)  EVIR.A.  EVLAN-11  Ethernet  Data  Link 

Controller  for  the  G64/G96  Bus/EVTCP  Internet  Package 
technical  manuals 

(2)  Internetworking  with  TCP/IP  Volume  I:  Principles, 
Protocols  and  Architectures,  Douglas  E.  Coiner, 

Prentice  Hall,  Englewood  Cliffs  NJ,  1991 

(3)  Internetworking  with  TCP/IP  Volume  II:  Design, 
Implementation  and  Internals,  Douglas  E.  Comer  and 

David  L.  Stevens,  Prentice  Hall,  Englewood  Cliffs  NJ,  1991 
(4;  IRIX  Network  Programirdng  Guide,  Silicon  Graphics  Inc. 
i ;  An  Advanced  4.3BSD  Interprocess  Communication  Tutorial, 
Samuel  J.  Leffler,  Robert  S.  Fabry,  William  N.  Joy, 

Phil  Lapsley,  Steve  Miller  and  Chris  Torek,  undated 
(6)  Real-Time  Programming  Tutorial,  Bill  Mannel ,  SGI  Expo, 
Silicon  Graphics  Inc.,  23  May  93 
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Aucnor : 


Don  Brutzman  brutzinan@cs  .nps  .navy  .mi  1 

Code  OR/Br 

Naval  Postgraduate  School  (408)  656-2149  work 

Monterey  CA  93943-5000  (408)  656-2595  fax 

Status:  Tested  satisfactory  Irix<==>Irix,  OS-9<==>OS-9 ,  Irix<==>OS-9 


Future  work:  Consider  implementation  as  an  inetd  'superserver'  daemon. 

Ensure  compatibility  with  DIS  v2 . 0  protocols  when  available. 


Comments,  suggestions  and  corrections  are  welcome! 


*ir***irirtc************-^-kicic*-k*****-k-k*ir*it*****it*ir-k*-k-k**if*-kic-*C**^***-k*iC***********-k*-k^ 

/♦  Irix  defines  in  /usr/include  */ 

#if  defined(sgi) 


#^r:clude 

<stdic . h> 

# include 

<sys/ types .h> 

^include 

<sy3/socket . h> 

include 

<net inet /in .h> 

# include 

<netdb . h> 

#incl ude 

<sigrial . h> 

^include 

-string , h> 

/-  OS-9  subdirectories  /DEFS,  /DEFS/INET  */ 
se 

^include  <stdio.h> 

<»anclude  -'types. h> 
i*::v:iudrr  -'inet/ socket  Ar> 

^include  <inet/in.h> 

^include  - inet /netdb . h> 

*  include  <signal.h> 

^include  <errno.h> 


^include  <time.h> 


/*  One  stream  socket  is  used  with  adequate  throughput  */ 

/*  (although  two  could  work,  no  performance  improvement  is  expected)  */ 

/*  Be  careful  that  you  reserve  these  port  numbers  to  prevent  collisions  */ 

/*  from,  other  processes  requesting  ports  on  your  system:  */ 

#define  DISBRIDGE_TCP_PORT  2056  /*  disbridge  program,  server  &  client  */ 

/*  NPS  Autonomous  Underwater  Vehicle  (AUV)*/ 
/*  Underwater  Virtual  World  (UVW)  */ 

#define  AUVSIM1_TCP_PORT_0  3210  /*  os9sender  <==>  os9server  test  programs  */ 
#define  AUVSIM1_TCP_P0RT_1  3211  /*  auv  execution  level  <==>  virtual  world  */ 

#define  AUVSIM1_TCP_P0RT_2  3212  /*  port  for  future  use  */ 

#define  AUVSIM1_TCP_P0RT_3  3213  /*  port  for  future  use  */ 

#define  AUVSIM1_TCP_P0RT_4  3214  /*  port  for  future  use  */ 

#define  AUVSIM1_TCP_P0RT„5  3215  /*  port  for  future  use  */ 

#define  AUVSIM1_TCP_P0RT_6  3216  /*  port  for  future  use  */ 

4fdefine  AUV£IM1_TCP_P0RT_7  3217  /*  port  for  future  use  */ 

#define  AUVSIMl_TCP_PORT_8  3218  /*  port  for  future  use  */ 

#define  AUVSIMl_TCP_PORT_9  3219  /*  port  for  future  use  */ 


#  define  TRUE  1 

#  define  FALSE  0 

#define  SOCKET_QUEUE_SIZE  5  /*  max  allowed  by  TCP/IP  */ 


function  prototypes 


ir**'*’****'*-*'*’'*'*****'Ji 


/*  Note  that  function  prototype  parameters  may  need  to  be  deleted  if  the 
C  compiler  used  is  not  an  ANSI  compiler  (e,g.  OS-9  K&R  C  compiler) 


void  shutdown__os9server  (); 


global  variable  definitions  ****************** *'*-***********^****************/ 

static  int  TRACE  =  0;  /*  1  =  trace  on,  0  =  trace  off  */ 

static  int  socket_descriptor , 

socket_accepted, 

30cket_stream; 

static  int  socket_length  =  256; 

static  int  by tes_received,  bytes_read,  bytes_wri tten, 

bytes_left,  bytes_sent; 

int  shutdown_signal_received; 

static  int  i  =0, 

print_help  =  FALSE; 


/*»***x********************»********:*r******************»*************-*****:J::*r***/ 

y*****-»*>r**********-**-*-******^***-****»****************^*^»*******************-***y 

main  (argc,  argv) 

int  argc;  cnar  **argv;  /*  command  line  arguments  */ 

char  command_sent  [61]  , 

command_received  [81] , 
remot€_host_name  [60] ; 

FILE  *netstat_f ileptr ; 

struct  sockaddr_in  serve r_address; 

register  struct  hostent  *server_enti ty ; 


char 


test_buffer  [81]; 


*ptr  ; 


y^***  +  *****************llr**********************************************^t*********y^ 

/*  Begin  execution,  parse  command  line  */ 


for  {i  =  1;  i  <  argc;  i++) 

{ 

if  {{argv[i][0]  !=  II  print_help) 


print_help  =  TRUE; 
break; 

) 

switch  {argv[i][l])  /*  switch  based  on  current  command  line  parameter  *! 
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case  ' h' :  /*  help  * / 

case  ' H ' :  /*  Help  * / 

case  ' ? ' :  /*  Help  * / 

print_heip  =  TRUE; 
break ; 


case  ' c '  : 
case  'T^  : 

TRACE  =  1; 
break ; 

default : 
break ; 


/*  TRACE  feature 


/*  no  coirmand  line  entry  parameter  present 


/*  end  switch  based  on  current  command  line  parameter  */ 
i*  end  for 


If  (print_help) 


/*  print  help  string 


printf  ("Usage:  os9server  [-help]  [ -trace] \n" ) ; 
exit  (- 1 ; ; 

if  (TRACE)  printf (" [os9server  TRACE  on]\n"); 
fflush  (stdin); 


romiriand  line  parameter  parse  complete 


★  ★★★Tt-St*****************'*'**  / 


Initialize  coirjriuni  cat  ions  blocks 


sccket_lenath 


Ket  length  =  81 


/*  maximum  allowed  packet  size  */ 


t*****»*******i 


Ir.icialize  both  client  S<  server  **.***.*  —  .*************'*•******”  —  —  ■■”--/ 

/*  Signal  handlers  for  termination  to  override  net_open  ()  and  net_close  ()*/ 
/*  siqnal  handlers.  Otherwise  you  are  unable  to  kill  this  program.  */ 


defined  (sgi  .i 

ignal  (SIGHUP,  shutdown_os9server) ; 
ignal  (SIGINT,  shutdown_os9server ) ; 
ignal  {SIGKILL,  shu tdown_os9sGrver ) ; 
lignal  (SIGPIPE,  shutdown_os9server ) ; 
tiqnal  (SIGTERM,  shutdown__os9server )  ; 


/*  hangup  */ 
/*  interrupt  character  */ 
/*  kill  signal  from  Unix  */ 
/*  broken  pipe  from  other  host  */ 
/*  software  termination  */ 


signau.  (SIGKILL,  snuraown^osi^ijei  vtii ;  ;  / 

signal  (SIGPIPE,  shutdown_os9server) ;  /*  broken  pipe  from  other  host  / 

signal  (SIGTERM,  shutdown__os9server) ;  /*  software  termination  */ 

4fendi  f 

/»»,»*^x*».**********»**********^»*’^******^***»^*******’"******^*^***************/ 

/*  Initialize  server  .*.**.*..  — —  ***********************“****‘*******/ 

/*  setup  to  listen  for  client  to  attempt  connection  */ 

’  /*  Server  opens  server  port  .*..*..*.*.»****»*******•**»»**•••*••**••**•*/ 

/*  Open  TCP  (Internet  stream)  in  socket  */ 

if  (  (socket^descriptor  =  socket  (AF_INET,  SOCK_STREAM,  0))  <  0  ) 

^  printf  {"os9server  can't  'open'  stream  socket*); 
exit  (-1) ; 

else  if  (TRACE)  ^  v  .  i 

printf  ("os9server  socket  'open'  successfulXn" ) ; 
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/ 


/*  Bind  local  address  so  client  can  talk  to  server  * 

#if  defined(sgi) 

bzero  ((char  *}  &fserver_address ,  sizeof  { server_address )  }  ; 

#endl  1 

serve readdress. sin_f ami ly  =  AF_INET;  /*  Internet  protocol  family  */ 


/*  make  sure  port  is  in  network  byte  order  */ 

server_address.sin_addr .s_addr  =  htonl  (INADDR_ANY) ; 
server_address. sin_port  =  htons  {AUVSIM1_TCP_PORT_0) ; 


if 


(bind  (  socket_descriptor , 

(struct  sockaddr  *)  &server_address , 

sizeof  (server_address ) )  <  0) 

print f ( “csPserver  socket  'bind'  unsuccessful \n" ) ; 
exit  {-!) ; 


] 

else  if  (TRA.CE)  printf  (“os9server  socket  'bind'  successful Xn"); 


/*  prepare  socket  queue  for  connection  requests  using  listen  */ 

listen  (socket_descriptor ,  SOCKET_QUEUE_SIZE} ; 

I f  (TRACE ( 

pri nt t ( “ os9server  socket  'listen'  successful  ...\n“); 


/*  Server  'accept'  waits  for  client  connections  ******^*********it***»****/ 

it  (TF.ACE,!  printf  r'os9server  socket  waiting  to  'accept'  ...\n''); 

by tes_recei ved  =  sizeof  ( socket_descriptor) ; 

wni^e  : {3ccket_accepted  =  accept  (  socket__descriptor ^ 

Scserver^address , 

Scbytes^received)  )  <  1)  /*  block  */ 

if  •: TRACE  i 
{ 

printf  ("os9server  socket  'accept'  unsuccessful,  "); 
printf  {‘'sleeping  1  second  ...Xn"); 
sleep  ( 1 ■ ; 

} 

printf  ( ‘'os9server  connection  is  open  between  networks  .Xn"); 


}  /  * 


/»■*■*•*  X  ik- 


IOOp 


end  initialization 

x*xxxx**x*xxxxxxx*****-x*xx**-xxx*x*x*****xx*x.*:***.*****.******^^^^^^^^^^ 

eransferring  telemetry  until  shutdown_signal_received : 


*/ 

*  *  y 
*/ 
**/ 


Two-way  reflector:  listen  to  local  program  and  relay  to  remote  host, 

listen  to  remiote  host  and  relay  to  local  program  */ 

socket_stream  =  socket_accepted;  /*  server  */ 

if  (TRACE) 


printf  ("os9server  SERVER:  socket_descriptor 
printf  ( "  socket_accepted 

printf  ( "  socket_stream 


%d,Xn'',  socket_descriptor)  ; 
%d, Xn" ,  socket_accepted) ; 
%dXn",  socket_stream) ; 


/x-*xx**x*»irx**x***** 

/*  test  handshakes 


**-*^******-i 


write  (socket_stream,  "SUCCESS  #1:  os9server  connected  to  os9sender'",  46); 
read  (socket_stream,  test_buffer,  46); 
test_buffer  (47]  =  'Xn'; 

printf  ("test  handshake  between  hosts:  \n%sXn",  test_buf fer) ; 


y  *****★**★***•★****★•*■ 


★*********************»**^*****^****^^ 


*****»**★***■»■*•*■*•**  y 
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/*  initialize  boolean  for  main  loop  control 
shutdown_signai_received  =  FALSE; 

while  {shutdown_signal_received  ==  FALSE)  /*  loop  until  shutdown..  TRUE  */ 

i 

/*♦»*■******★*♦*»**************★»**•★•*•★★•*:*******★★*★*★**«**★**★*★**■**★★*★*■****/ 

/*  Receiver  block  */ 

^/♦★**»**»»*»*********»-***^^^^***********************-*:******<r*******»*^****-**/ 

/*  listen  to  remote  host,  relay  to  local  network /program  */ 

byces_left  =  socketjength; 

by tes_received  =  0; 

pr r  =  command_received; 

while  ( (bytes_left  >  0)  &&  (by tes_received  >=  0))  /*  read  loop  ************/ 

{ 

bytes_read  =  read  ( socket_stream,  ptr,  bytes_left); 

if  {bytes_read  <  0}  bytes_received  =  bytes_read; 

else  if  (bytes_read  >  0) 

/ 

bytes_left  -=  bytes_read; 

by tes_received  +=  bytes_read; 
ptr  +=  bytes_read; 

} 

if  (TRACE) 

printf  (“os9server  receiver  block  loop  bytes_read  =  %d\n*' , 
bytes_read)  ; 

/*  if  nothing  is  waiting  to  be  read,  break  out  of  read  loop 
if  f {bytes_read  ==  0}  &&  {bytes_received  ==  0))  break; 


if  (by tes_received  <  0)  /*  failure  */ 

if  (TRACE) 

{ 

printf  {'*os9server  receiver  block  read  failed,  “ }  ; 
printf  ( *' by  tes_received  =  %d\n",  bytes_received)  ; 

} 

} 

else  if  (by tes_recei ved  ==  0)  /*  no  transfer  */ 

{ 

if  (TRACE) 

print f  ( '’os9server  get_PDU_f rom_other_host  read  received  0  bytesNu"); 

) 

else  if  (bytes_received  >  0)  /*  success  */ 

printf  ("%s\n'',  command_received)  ; 

/•»*’-*-***************»**T»>'*:*****»*****»****»****»************************^^***»/ 

/*  Check  termination  */ 

/★*♦*******★★*★*****************★*****★•******•***★****★★*★****************★♦*  ^ 

if  (strncmp  (command_received,  “shutdown",  8)  ==  0) 
shutdown_signal_received  =  TRUE; 

if  ( shutdown_signal_received  ==  TRUE) 

t 

shutdown_os9server  ( ) ; 
break; 

} 

/★**Tl-*-*********'**^******<-********************^***********5fc*****-*******it*****/ 
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*****ic-k-*;******iriir*rk******ieie*'k-k-kie*-kic**ie****-k**ir****itie*ie*ir 


/ 

/ 


/*  Sender  block 

y/-**»r*r»**********»-*'* 

/*  read  from  local  stdin,  relay  to  remote  host  */ 

gets  ( comma nd_sent ) ; 

byces_received  =  strlen  (coininand_sent) ; 

if  (TRACE)  printf  ( " [os9server  command_sent : %s] Xn" ,  comraand_sent ) ; 

if  (by tes_received  <  0)  /*  read  failure  */ 

( 

printf  (“os9server  gets  ()  from  keyboard  unsuccessfulXn”); 
shutdown_os 9 server  { ) ; 


if  (bytes_received  >  socket_length) 

{ 

printf  (’'os9server  send_telemetry_to_server  error:  “); 
printf  ( '■bytes_received  too  big  for  packet  socket_length\n " )  ; 
printf  ( ••); 

printf  i  "  [by tes_received=%d]  >  [ socket_length=%d]  ;  , 

by tes_rec6ived,  socket^length) ; 

printf  ("string  trunca tedXn" ) ; 


bytes_left  =  socket_length; 

by tes_wri tten  =  0; 

pci'  =  comm.and_sent  ; 

while  ( (bytes_left  >  0)  &&  (by tes_writ ten  >=  0))  /*  write  loop  ***********/ 

bytes_sent  =  write  (socket^stream,  ptr,  bytes_left); 

if  (bytes__sent  <  0)  by tes_wri tten  =  bytes_sent; 

else  if  (bytes_sent  >  0) 

( 

bytes_left  -=  bytes_sent; 
bytes_written  +=  bytes_sent; 
ptr  +=  bytes_sent; 

} 

If  (TRACE) 

printf (" os9server  send_telemetry_t observer  loop  bytes  sent  =  %d\n", 
by tes_sent ) ; 


if  (by tes_writ ten  <  0) 

{ 

printf  ("os9server  send_telemetry_to_server  ()  send  failed,  "); 
printf  (‘'%d  by tes_wri ttenXn" ,  bytes_written)  ; 

} 

else  if  (TRACE) 

printf  ("os9server  send_t el emetry_t observer  total  bytes  sent  =  %d\n'', 
by tes_written) ; 

/***ir***tcir-k*****ic*****************i:*****icic^***-k±*:tr*^*^*ic***-kic**i,ic***i,ir*ir***ic/ 

/*  Check  termination  */ 

/*****-k-k********t:**tr**-k*-kic**-k**********-k*****-k*ir*-k**iri:ic*iricic*ic**i,tc*ic*1c*icicicfk*jf 

if  (strncmp  { comma nd_s en t ,  "shutdown",  8)  ==  0) 
shutdown_signal_received  =  TRUE; 

if  (shutdown_signal_received  ==  TRUE) 

{ 

shutdown_os9server  (); 
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break ; 


)  /*  end  of  while  ( shutdown_signal_received==FALSE)  indefinite  loop  */ 

.?nutdowr_cs9server  i);  /*  ensure  shutdown_os9server  is  always  reached  */ 

print f  (‘'os9server  exitAn"); 

exit  (0);  /*  os9server  complete  */ 

\ 

^■■^■K*-x-r*ir^i[*-k***:»:ic-K***-K*ic****icieir**-k****ie**-tc-k****'k*******************************/ 

^*ieieir-^ir*ir***********ir**********-k****-k*-kieic**'k*-k*iiitirt;*'ie***ic******ic**ic****ic****ie**^ 

/*  end  of  main  proaram 

/***-ir-^*-^-^******ic**lir*y^ir‘>r**********ir**’k*ic*-kic*********-k’k*****ic-k*±ic*it****-k*icicicic-k**/ 

'^‘*^*ir*-t*-ri^********-k****************icic***ic*-k*tc-k-k**icic****‘k*-k*******-k***-k********’k/ 

Shutdown  block 
void  shutdown_os9server  {) 

if  (TRACE)  printf  ("osSserver  shutdown  in  progress  ...Xn"); 

shutdown_signal_received  =  TRUE;  /*  in  case  entry  was  from  signal  handler  */ 

/*  No  need  to  send  a  message  to  other  side  that  bridge  is  going  down,  _  */ 

/*  since  SIGPIPE  signal  trigger  may  s hutdown_os 9 sender { )  on  other  side?  */ 

if  ‘Close  (socket_stream)  ==  -1) 

printf  ("osPserver  close  { socket_stream)  failedXn"); 

if  (TRACE)  printf  ("os9server  shutdown_os 9 server  ()  completeXn " ) ; 

return ; 

}  /*  end  shutdown_os9 server  ()  */ 

.^^^^^^^^^yc*-K->,-*r-x-x-xy^ir*-^*->,*V****-K-k->c******-k*-K*if*-k-K-k-KiC*******:k*******-k******ic****-k*^ 
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D. 


disbridge.c  LAN  to  LAN  Connectivity  for  DIS  without  Multicast 


/x*'»r***'3rTrTr-*-*ir*'*-***'**'*-**7e*******'*-Sk-1l:************-*:***********-********************** 

P  r  og  1'  an  I :  disbridge.c 

Relays  DIS  packets  between  LANs  on  different  Internet  hosts 
7  July  93  DIS  version  1.3 
%  DIS-1.3/src»  make  bridge 
%  hostl»  disbridge  -server 

%  hosL2>>  disbridge  [-client]  -remote  hostl . internet . address 

Execution  options: 

_c  -c  -r  -H  -I  -t  (first  letters  are  sufficient,  capital  letters  are  OK  too) 
-server  server  mode;  must  be  running  prior  to  client  calling  for  it 

f-client]  client  mode  [default];  remote  host  must  be  specified  and 
have  disbridge  -server  already  running  there 

-remote  hostname  hostname .  remicte  .  net .  address  for  client  to  connect  to  server 
-help  display  calling  syntax 

-interface  etO  will  force  use  of  a  given  device  interface  (e.g,  "etO")  for 
multicast  packet  listening  on  the  local  net.  disbridge  will 
attemipt  to  open  the  first  known  device  if  the  default  device 
fails.  Only  one  parent  process  is  allowed  to  use  that 
device  on  a  given  machine.  Thus  if  you  want  to  connect  one 
local  area  network  (LAN)  with  another,  each  LAN  must 
dedicate  one  machine  to  disbridge.  This  appears  to  be  an 
unavoidable  hardware/mul ticast  restriction. 

-trace  turn  on  TRACE  mode 


References:  (I)  Military  Standard--Protocol  Data  Units  for  Entity 

Information  and  Entity  Interaction  in  a 
Distributed  Interactive  Simulation  (DIS), 

Institute  for  Simulation  and  Training,  University  of 
of  Central  Florida,  Orlando  FL,  30  OCT  91 

(2)  Internetworking  with  TCP/IP  Volume  I:  Principles, 
Protocols  and  Architectures,  Douglas  E.  Comer, 

Prentice  Hall,  Englewood  Cliffs  NJ,  1991 

(3)  Internetworking  with  TCP/IP  Volume  II:  Design, 
Implementation  and  Internals,  Douglas  E.  Comer  and 

David  L.  Stevens,  Prentice  Hall,  Englewood  Cliffs  NJ,  1991 

(4)  IRIX  Network  Programming  Guide,  Silicon  Graphics  Inc. 

(5)  An  Advanced  4.3BSD  Interprocess  Communication  Tutorial, 
Samuel  J.  Leffler,  Robert  S.  Fabry,  Williairi  N.  Joy, 

Phil  Lapsley,  Steve  Miller  and  Chris  Torek,  undated 

(6)  Real-Time  Programming  Tutorial,  Bill  Mannel,  SGI  Expo, 
Silicon  Graphics  Inc.,  23  May  93 

(7)  DIS  vl.2  source  library,  John  Locke,  Naval  Postgraduate 
School,  Monterey  CA,  8  FEB  92 


Description : 
Revised: 
Compilation : 
Execution : 
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Author : 

AcKiiOwledgement : 
assistance , 

Status: 
protocol . 

Save at : 
ai't  icuiated 

pointers 

socket . 

running 


Don  Brutzman  brutzinan@cs.nps.navy.mil 

Code  OR/Br 

Naval  Postgraduate  School  (408)  656-2149  work 

Monterey  CA  93943-5000  (408)  656-2595  fax 

Thanks  to  John  Locke,  Mike  Macedonia  &  Dave  Pratt  for 

Tested  satisfactorily  for  EntityStatePDUs  using  vl.2  DIS 

Currently  transmits  EntityStatePDUs  after  removing 
parameters.  This  is  due  to  articulated  parameter 
not  surviving  transmission  of  the  PDU  image  over  the 

Testing  to  date  has  been  exclusively  on  SGI  wrkstations 

Irix  4.0.5E  operating  system 

Consider  implementation  as  an  inetd  'superserver'  daemon. 

Ensure  compatibility  with  DIS  v2 . 0  protocols  when  available. 

Consider  conversion  from  iterative  to  concurrent  processing 
if  throughput  performance  needs  improvement. 

Initialize  opposite  hosts  using  ActivateRequest  & 

ActivateReply  exchanges,  as  well  as  DeActivateRequest  & 
DeActivateReply  exchanges. 

Comments,  suggestions  and  corrections  are  welcome! 


t*trtrirtcfc*te*'xir**tr**tr-*:fe***-k************ic*iiicirir-)t*-k-Kic-t;ic'k-k**-t**************-ic**********/ 

^include  "disdefs.h"  /*  DIS  Library  */ 


# include  '-'netdb.h'> 

4*  i nci ude  <riet  inet  /in  .  h'* 

#  include  '^sy s  .-'socket  .  h> 
i^include  --stdic.hv 
^include  -'tirrie.h> 

#  include  <sys./ types  .  h> 


/*  One  stream  socket  is  used  with  adequate  throughput  */ 

/*  (although  two  could  work,  no  performance  improvement  is  expected)  */ 

/’  Be  careful  that  you  reserve  these  port  numbers  to  prevent  collisions  */ 

/*  from  other  processes  requesting  ports  on  your  system:  */ 

#define  DISBRIDGE_TCP_PORT  2056  /*  disbridge  program,  server  &  client  */ 

/*  NPS  Autonomous  Underwater  Vehicle  (AUV)*/ 
/*  Underwater  Virtual  World  (UVW)  */ 

#define  AUVSIM1_TCP_PORT_0  3210  /*  */ 

tdefine  AUVSIMl_TCP_PORT_l  3211  /*  for  future  use  */ 

#define  AUVSIMl_TCP_PORT_2  3212  /*  */ 

#define  AUVSIM1_TCP_P0RT_3  3213  /*  */ 

#define  AUVSIM1_TCP_P0RT_4  3214/*  */ 

#deflne  AUVSIM1_TCP_P0RT_5  3215  /*  */ 

^define  AUVSIMl_TCP„PORT_6  3216  /*  */ 

^define  AUVSIMl_TCP_PORT_7  3217  /*  */ 


^define  AUVSIM1_TCP_P0RT_8 
#define  AWSIM1_TCP_P0RT_9 


#def Ine  SOCKET_QUEUE_SI ZE  5 
#dp-:lne  SOCKET_PADDING  10 


3218  /’ 

3219  n 


/*  max  allowed  by  TCP/IP  */ 

/*  a  little  extra  buffer  after  each  PDU  */ 


/*  function  prototypes  *********************-»*************it******^*************/ 

/*  Note  that  these  function  prototypes  may  need  to  be  commented  out  it  the 

C  compiler  used  is  not  an  ANSI  compiler.  */ 


void  shutdown_disbridge 
void  send_PDU_to_other_host 
i n  t  g e  t _  P DU_ f  r  om_o  the  r_h  o s  t 
vo:d  priijt_PDU 


(const  int  status); 

(char  *send_PDU,  PDUType  send_type) ; 
(char  **get_PDU,  PDUType  *get_type) ; 
(char  *pdu,  PDUType  type) ; 


/*  global  variable  definitions  *********»****»*************★*★*★****★*****★****/ 

static  int  TRACE  =  0;  /*  1  =  trace  on,  0  =  trace  off  */ 

static  int  socket_descriptor , 

socket_accGptGd, 

socket_stream; 


/*  maxiirium  arrowed  socket  length  value  =  ETHERMTU  [ /sys/netinet/ifether  .h] 
/*  see  initialization  code  for  actual  value 

static  int  socket^length  =  ETHERMTU; 

static  int  bytes_recGived; 


static  char 
static  PDUType 


*  local__PDU; 

1  oca l_PDU_type ; 


static  char  *remote_PDU; 

static  PDUType  remote_FDU_typ ec¬ 


static  int 


CLIENT,  /*  boolean  variables  */ 

SERVER; 


*/ 

*/ 


/*  John  Locke's  carried-over  variable  definitions  ****^*****************^-*'*****/ 


int  i  =0, 

print_help  ==  FALSE, 

ethernet_f lag  =  FALSE, 

nodes , 
read_f lag, 
recvd  =  0 , 

sent  =  0, 

write_f lag; 


extern  unsigned  short  host_id; 

char  *pdu, 

ethernet_interface  [MAX_INTERF+1 ] ; 


PDUType 


type; 


in  large,  argv) 

int  arge;  char  **argv;  /*  command  line  arguments  */ 

char  command  [81]/ 

new_device_name  [ 4 ]  , 
remote_host_name  [60]; 

inc  shutdown; 

FILE  *netstat_f iieptr; 

struct  sockaddr_in  server_address ; 

giscer  struct  h  os  tent  *server__entity  ; 

char  test_buffer  [37]; 

*»*»■*:  ****»*T»r*»****»*******-*^*********Tll:*************-***^**^**********W»*******/ 

Begin  execution,  parse  command  line 

r>efault  execution  is  server  waiting  for  client  request  to  read/write  PDUs  */ 

SEFVEr  =  TRUE; 

CLIENT  =  FALSE; 

for  {i  =  1;  i  <  arqc;  i++} 

{ 

if  ({argv{i][0]  !=  M  print_help) 

print_hGlp  =  TRUE; 
break ; 

switch  (argv[i}[l])  switch  based  on  current  command  line  parameter  */ 

case  ' h" :  / *  help  ^ / 

case  '  H'  :  t Help  */ 

case  ' ? ' :  / "  Help  * / 

print_help  =  TRUE; 
break ; 

case  'i':  /*  Ethernet  interface  */ 

case  'I':  /*  Ethernet  Interface  */ 

ethernet_f 1 ag  =  TRUE; 
if  ;i-^l  <  argci  { 

if  (strlen (argv[i+l] )  >  MAX_INTERF)  { 

printf(’'%s:  error:  interface  name  exceeds  %d  charsNn", 

a  rgv [ 0 ] ,  MAX_INTERF } ; 

exit  ( “ 1 ) ; 

}  else  ( 

strepy  (ethernet_interf ace,  argv[++i]); 

} 

)  else 

print_help  =  TRUE; 
break; 

case  'c':  /*  client  initiates  server  for  reading/writing  PDUs  */ 

case  ' C : 

CLIENT  =  TRUE; 

SERVER  =  FALSE; 
break; 

case  'r':  /*  remote  hostname  tells  client  who  to  connect  to  */ 

case  ' R' : 

CLIENT  =  TRUE; 

SERVER  =  FALSE; 

/*  get  remotehost  name  */ 

if  {i+1  <  arge)  strepy  (remote_host_name/  argv[++i]); 
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else 

break; 


print_help  =  TRUE; 


/*  server  waits  for  client  request  to  read/write  PDUs  */ 

FALSER- 

TRUE; 


case  'c':  /*  TRACE  feature  V 

case  'T' : 

TRACE  =  In¬ 
break; 


CaStr  t>  : 

CLIENT  = 
SERVER  = 
break; 


def aul t : 

print_help  =  TRUE; 
}  /*  end  switch  */ 

}  /*  end  for  */ 


if  (argc  ==111  print_help)  /*  print  help  string  •»-***************^*********/ 
printf  ("Usage:  disbridge  [-client  -remote  hostname  I  -server  <default>] 


printf  ("  [-i  device#]  [-help]  [- trace] \n ") ; 

exi t  ( - 1 ) ; 


printf  ( " \n" ) ; 

if  1 ! ethernet_f lag)  strcpy  {ethernec_interf ace,  BCAST_INTERF) ;  /*  default  */ 

if  (net_open  ( ethernet_interf ace)  ==  FALSE) 
i 

printf  ("disbridge  net_open  {\'’%s\")  failed,  \n",  ethernet_interface)  ; 
printf  ("  due  to  improper  device  number  or  device  is  already  in  use\n"); 
net_close  ();  /*  clean  up  after  a  deficient  net_open  ()  DIS  function  */ 

/*  this  recovery  procedure  would  be  better  located  there  */ 

/"  Show  user  what  correct  port  number  is  */ 

if  (TRACE) 


sprintf  (command,  "netstat  -I"); 
printf  (  "netstat  -I\n"); 

systeiTi  (command)  ;  /*  display  results  of  netstat  -I  command  */ 


sprintf  (command,  "netstat  -I  >  netstat. I ") ; 
systemi  (command)  ; 

if  ( (netstat_fileptr  =  fopen  ( "netstat . I " ,  "r"))  ==  (FILE  *)  NULL) 

{ 

fprintf  (“disbridge  failed  to  create  local  file  netstat . I \n" ) ; 
shutdown_disbridge  (1); 

) 

/■*■  flush  first  line  then  read  first  device  name  from  file  netstat.  I  */ 

while  (command  [0]  !=  '\nM  fscanf  {netstat_f ileptr ,  "%c",  &command[0] ) ; 

fscanf  (netstat_fileptr,  "%3s“,  new_device_name) ; 

fclose  (netstat_f ileptr) ; 

sprintf  (command,  "rm  netstat. I") ; 

system  (command) ; 

/*  Fixed  problem  in  the  DIS  library,  bad  net„close()  call  caused  exit  */ 
strcpy  (ethernet_interf ace,  new_device_name) ; 
if  (net_open  (ethernet_interf ace)  ==  FALSE) 

{ 

print f ( "disbridge  net_open  (\"%s\")  failed, \n“,  €thernet_interface) ; 
printf ("  improper  device  number  or  device  already  in  use\n"); 
exit  (-1) ; 
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) 

} 

print  f  (“disbridge  net_open  (\’'%s\”)  succeededXn" ,  ethernet_interface)  ; 
CoTTUTiand  line  parameter  parse  complete  */ 

/»**»**»-»**»*******it***'*********»***x**T»r******»********iir****1t*^***Hr**'»-*1»:**»****y^ 

/*  Initialize  communications  blocks  */ 

/*****-»!ir*****-kir-k**ie***ie-x*-ir**-k***ie**’kicic'tr‘k**-k-ic*-kie*ie*-ic***'kieie±-k-kic-k**-k'kt**ic*ic**ic**-k*^ 

/*  set  socket_length  */ 


/* 


socket_Iength  =  ETHERMTU;  */  /*  maximum  allowed  packet  size  * 

socKet_lenqth  =  sizecf  ( EntityStatePDU)  +  SOCKET_PADDING; 

Note  that  DIE  protocol  version  2  will  convert  'unused'  to  PDU  length  field  * 


★  Hr'*-'*'*-'*-**'**’**'*'*****'*'********'*’*'** 

Initialize  both  client  &  server 


★  *•★»★***★**»*******★**•★•*•**»★*»♦***★★★*★★**** 
******iit^**;lr***n:**Kr**»**********'**»****itit»*** 


/ 

/ 

/ 

/ 


/*  Signal  handlers  for  termination  to  override  net_open  {}  and  net__close  ()*/ 
/*  signal  handlers.  Otherwise  you  are  unable  to  kill  this  program.  */ 

signal  (SIGHUP,  shutdown_disbridge) ;  /*  hangup  */ 

signal  {SIGINT,  shutdown_di sbridge) ;  /*  interrupt  character  */ 

signal  (SIGKILL,  shutdown_di sbridge) ;  /*  kill  signal  from  Unix  »/ 

signal  ^SIGPIPE,  shutdown_disbridge) ;  /*  broken  pipe  from  other  host  */ 

Signal  .SIGTERM,  shutdown_disbridge)  ;  software  termination  */ 

/*  Future  work:  implement  inter-network  initialization  by  sending 

Sendz^ctivateRequestPDU' s  for  all  entities  on  the  local  net 
to  first  infcriT:  the  remote  host  of  their  existence  */ 

/’■  not  implemented  -  PDU's  copied  when  received  regardless  of  activation  */ 


■.  ^•>:*yr-riricir*-rTt:-kir^t***-x**ir****-»r*-k****************-kic**ir-t*ir*-k*****it**ir***-k**ir*-kic*  j 

/■’  Initialize  client  ******'*■**»*******»**»»**»****★**»★*********★*♦*★******»***/ 

If  (CLIENT)  /*  start  by  finding  remote  host  to  connect  to  */ 

{ 

serve r__en t i  ty  =  gethostbyname  { remote_host_name )  ; 
if  :  s  e  r  ve  r_en  i  ty  =  =  NULL ) 


I'  ^  ^  - 
exi  t 


disbridge  -remote  host  (\"%s\")  not  foundXn" 
r emtO  t  e_h os  t _n ame )  ; 


1 

else  if  'TRACE) 

printf ( “disbridge  remote  host  (\“%s\“)  locatedXn", 


remote_host_name)  ; 


/*  Client  opens  server  port  *^*************»*****************^**^***^****/ 


/*  Fill  in  structure  'serve readdress'  with  the  address  of  the 

remote  host  (i.e.  SERVER)  that  we  want  to  connect  with  */ 

bzero  {{char  *)  Scserver_address ,  sizeof  {server_address) ) ; 
server_address.sin_family  =  AF_INET;  /*  Internet  protocol  family  */ 

/*  copy  server  IP  address  into  sockaddr_in  struct  serve readdress  */ 

bcopy  (server_entity->h_addr,  &{  serve  readdress,  si  n_addr.s_addr) , 
server_entity->h_length) ; 

/*  make  sure  port  is  in  network  byte  order  */ 

server_address . sin_port  =  htons  (DISBRIDGE_TCP_PORT) ; 
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/*  open  TCP  (Internet  stream)  socket  */ 

if  {  ( socket_descriptor  -  socket  (AF_INET,  SOCK_STREAM,  0))  <0  ) 

{ 

printf  ("disbridge  client  can't  open  server  stream  socket"); 
exi t  (  -1  •  ; 

) 

6 i s e  if  ( TRAl E ) 

printf  (“disbridge  client  open  server  socket  successfulXn" ) ; 

/*  Connect  to  the  server.  Process  will  block/sleep  until  connection  is 

is  established.  Timeout  will  return  an  error.  */ 

if  (connect  (  socket_descriptor, 

(Struct  sockaddr  *)  &server_address , 

sizeof  (server_address) )  <  0) 

{ 

printf  (“disbridge  client  can't  connect  to  server  socketXn"); 
exi t  (-1 ) ; 


else  if  (TRACE) 

printf ( “disbridge  client  connect  to  server  socket  successfulXn" 
end  if  (CLIENT)  initialization 


utialize  server 


eit**********-********^ 


^*★■*★■*★*■**★■*■*11 


/*  setup  to  listen  for  client  to  attempt  connection 


/*  Server  opens  server  port  *****************************^»**--*^******’ 

/*  Open  TCP  .Internet  stream;-  in  socket 

If  {  -socket_descriptcr  =  socket  (AF^INET,  SOCK^STREAM,  0))  <  0  ) 


printf  (“disbridge  server  can't  'open'  streami  socket”); 
exit  f-1); 


printf  (“disbridge  server  socket  'open'  successful \n “) ; 

/’  Find  local  address  so  client  can  talk  to  server  */ 

bzerc  i (char  &server_address ,  sizeof  (server_address) ) ; 

£erver_address  .  3in_f  ami  ly  =  AF_INET;  /*  Internet  protocol  family  '^  / 

cerver_address.sin_addr .s_addr  =  htonl  (INADDR^ANY) ; 
server_address . sin_port  =  htons  (DISBRIDGE_TCP_PORT) ; 

if  (bind  (  socket_descriptor , 

(struct  sockaddr  *)  &server_address , 

sizeof  (server_address) )  <  0) 

{ 

printf ( “disbridge  server  socket  'bind'  unsuccessfulXn” ) ; 
exit  (-1 ) ; 

} 

else  if  (TRACE)  printf  ("disbridge  server  socket  'bind'  successfulXn" ) ; 

prepare  socket  queue  for  connection  requests  using  listen  */ 

listen  (socket_descriptor,  SOCKET_QUEUE_SIZE) ; 
if  (TRACE) 

printf ( “disbridge  server  socket  'listen'  successful  ,..Xn"); 

/*  Server  'accept'  waits  for  client  connections  *******************♦*****/ 

if  (TRACE)  printf  (“disbridge  server  socket  waiting  to  'accept'  ...Xn"); 

bytes_received  =  sizeof  {socket_descriptor) ; 

while  (  (socket__accepted  =  accept  (  socket_descriptor , 

&server_address , 

&bytes_received) )  <  1)  /*  block  */ 
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rt 


if  (TRACE; 

printf  ("disbridge  server  socket  'accept'  unsuccessful,  "1 
printf  (“sleeping  1  second 
sleep  (1); 

pri ntf ( “disbridge  connection  is  open  between  networks .\n“); 


}  /*  end  if  (SERVER)  initialization 


k**************-************  / 


loop  until  shutdown: 


f**************-#***************************/ 


/*  Two-way  reflector:  listen  to  local  network  and  relay  to  remote  host, 

listen  to  remote  host  and  relay  to  local  network  */ 

if  (SERVER'  socket_stream  =  socket_accepted; 

else  /*  CLIENT  */  socket^st ream  =  socket_descriptor ; 

:f  ^ TRACE) 

s 

if  (CLIENT)  printf  ("disbridge  CLIENT:  “); 

(SERVER)  printf  ("disbridge  SERVER:  " ) ; 

-.rintff  *  "socket_descriptor  =  %d,\n",  socket_descriptor) ; 

printf  (*'  socket_accepted  =  %d,\n",  socket_accepted)  ; 

printf ("  socket_stream  =  %d\n",  socket_stream) ; 


t***********-**^*»'*********’ 


:est  handshakes 


(CLIENT'- 

'write' ’{sccket_stream,  "disbridge  CLIENT  connected  to  SERVER",  36); 
if  (SERVER) 

write  (socket_stream,  "disbridge  SERVER  connected  to  CLIENT",  36); 
read  ( socket_stream,  test_buffer,  36); 
e3t_buffer  [37]  =  '\n'; 

rintf  ("test  handshake  between  hosts:  %s\n",  test_buffer) ; 


After  this  point  the  behaviors  of  the  CLIENT  and  SERVER  are  identical 
^"sc  we  no  longer  bother  testing  for  which  version  is  being  executed  */ 


shutdown  =  FALSE; 

. ile  (shutdown  ==  FALSE; 


/*  initialize  boolean  for  main  loop  control  */ 
/*  begin  to  loop  indefinitely  until  shutdown  met  */ 

r-K-x -A  *-K*frfC***'X************'^*'^**********************/ 


/*  Sender  block 


rlric*************'*'*’'^***^ 


/*  read  local_PDU  from  local  network,  relay  it  to  remote  host  */ 

nodes  =  net_read  (ilocal_PDU,  Sclocal_PDU_type) ;  /*  note  pointers  passed  */ 

if  (nodes  <  0)  /*  net_read  failure  */ 

*  printf  (“disbridge  net_read  ()  of  local  PDU  traffic  unsuccessfulXn"); 
shutdown_disbridge  (1); 

else  if  (nodes  >  0)  /*  at  least  one  PDU  found,  print  it  if  in  TRACE  mode  */ 

{ 

if  (TRACE) 

^  printf  ("disbridge  net_read  ()  local_PDU  captured  successfully :\n"); 
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print_PDU  (local_PDU,  local_PDU_Cype) ; 

} 

/*  PDU  found.  No  need  to  check  origin  tag  prior  to  relaying  it  since 
LJS  library  will  ensure  that  net_read  ()  does  not  see  PDUs  that 
originated  from  the  self  process's  net_write  ().  Such  checks  are 
essential  at  some  level,  or  a  pathological  race  condition  will  develop 


and  possibly  swamp  both  networks  with  duplicated  PDU  traffic.  */ 

send_PDU_to_other_host  (local_PDU,  local_PDU_type) ; 

else  / *  nodes  ==  0,  no  pdu  found,  no  action  required  except  trace  */ 

if  (TRACE) 

printf  ('‘disbridge  net_read  {)  no  local  PDU  traffic  pending  ...\n"); 

y»ir*»*»*»»»*******-**************»***********************************^t*******y 

/*  Receiver  block  */ 

******************★*****,  *********************^f*********************<:***^ 

/*  listen  to  remote  host,  relay  remote  PDU  to  local  network  */ 

/*  if  remote_?DU  found,  net_write  it  */ 


if  ( get_PDU_f rom_other_hos t  (Scremote_PDU,  &remote_PDU_type )  >  0) 
if  (TRACE) 

printf  ("disbridge  get_PDU_f rom_other_host  successful ; \n ") ; 
print_PDU  (remote_PDU,  remote_PDU_type) ; 
printf  ("disbridge  calling  net_write  ()  "); 
printf  ("to  issue  remote_PDU  locally  ...\n"); 

if  (net_write  iremote_PDU,  remot€_PDU_type )  ==  FALSE) 

printf  {"disbridge  net_write  ( remote^PDU)  unsuccessful \n ") ; 

else  if  (TRACE) 

printf  ("disbridge  net_write  (remote_PDU)  successful \n ; 
else  if  (TRACE)  printf  (“disbridge  g€t_PDU_f rom_other_host  unsuccessfulXn" ) ; 

yTx»*-r****XKr>-»*****»**»»*****»ilr»,r-*-*»**************************»***^r*#^******y 

/’■  Check  termination  */ 

/rxxxxx*xx***x*xx**x»x**************x********************************.*******y 

if  (shutdown)  shutdown_disbridge  (0) ; 

if  (TRACE) 

{ 

printf  (“disbridge  no  PDUs  pending,  sleep  (1)  . . . \n“ ) ; 
s 1 eep  ( 1 )  ; 

} 

/*  shutdown  =  TRUE;  */ 

}  end  of  while  (shutdown==FALSE)  indefinite  loop  */ 

shutdown_disbridge  (0);  /*  ensure  shutdown__disbridge  is  always  reached  */ 

exit  (0);  /*  disbridge  complete  */ 

} 

/*-k****-k***********-k***-k**********-k****ic******-^ic^ic-k**ir1c***-k-kic*-k*i(i,i,iii,i,i,i,^^^^^,^^^ 

/*X**********-****************-*********C****ir**************************^**^^^^^^^ 

/*  end  of  main  program  */ 

j'***-k*-k-k******tr^**ir**fk******-k**-ic*ic*ic*****-k‘kic******-kic***i,***i,ifkic*ic-ici,i,icici,icic±i,i,ici,^ 

/★xxxx******^**************************-****************^*************^^^^^^**^^*^ 

/******************************************************************^^**^*^^*^^*^ 
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/*  Shutdown  block  > 

ft!************-k'k**-irir-isic-k-k*-k-k-k*********ic'k*-k**-k****i(ic**’kiric*i(***************'*^******l 


void  shutdown_disbridge  (status) 
const  int  status; 

{ 

It  (TRACE)  printf  {‘'disbridge  shutdown  in  progress  ...\n"); 

/*  No  need  to  send  a  message  to  other  side  that  bridge  is  going  down, 

since  SIGPIPE  signal' triggers  shutdown_d is bridge  ()  the  other  side  */ 

if  (close  (socket_stream)  ==  -1) 

printf  ("disbridge  close  ( socket^stream)  failedXn"); 

net_close  ();  /*  close  local  net  port  connection  listening  for  DIS  PDU's  */ 
/*  watch  out  for  net_close  side  effects/interrupts  disabled!  */ 

orinrf  ("disbridge  exit,\n"); 

exit  (status);  /*  status  parameter  indicates  normal  or  abnormal  exit  */ 

)  /  *  e  n  a  s  h  u  t  d  o  w  _d  i  s  b  r  i  d  g  e  ( )  *  / 


/ 


*tc**ir*tc**i(tc*-**-k**-kic-k****-k*-k*’k**^-k*******************'^'*^/ 


void  send_PDd_to_other_host  (send^PDU,  send_PDU_type ) 

char  *send_PDU; 

PDUType  send_PDU_type; 

int  i,  bytes_written ,  bytes_left,  bytes_sent; 

char  *ptr; 

Enti tyStatePDU  *cast_PDU; 

char  FDU_character_buf fer  [ETKERMTU] ;  /*  ETHERMTU :  /sys/netinet /if ether .h  */ 

if  { send_PDU_type  ==  Ent i tyStatePDU_Type} 

{ 

/*  prevent  passing  articulated  parameter  ponters 

cast_PDU  =  ( Enti tyStatePDU  *)  send_PDU; 
cast_PDU->num_articulat_params  =  0; 

bcopy  (send^PDU,  PDU_character_buf f er,  socket_length) ; 
if  (TRACE) 

pRintf  ("disbridge  send_PDU_to_other_host  socketjength  =  %d\n*', 
socket_length ) ; 

printf  ("disbridge  send_PDU  type:  %d\n",  send_PDU_type) ; 
printf  ("disbridge  send_PDU  character jDutter  =  [ " ) ; 
for  (i=0;  i  <  socket_length;  i  +  -^) 

printf  (‘'%x  ",  PDU_character_buf f er  [i]); 
printf  ( " ] \n" ) ; 

printf  ("disbridge  send_PDU  character Jbu ft er  =  [ " ) ; 
for  (i=0;  i  <  socket_length;  i++) 

printf  ("%c",  PDU_character__buf fer  [i]); 
printf  ("]\n"};  f flush  (stdout); 

} 

if  (socket_length  >  ETHERMTU) 

^  printf  ("disbridge  send_PDU_to_other_host  error:  “ ) ; 
printf  ("PDU  too  big  for  packet"); 
printf  {"  "); 

printf  ("[PDU  socket_length=%d]  >  [ETHERMTU=%d] ;  nothing  sentXn", 
socket_length,  ETHERMTU) ; 

return; 
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T5  cr 


} 

byLes_left  =  sockec^length; 

by  ten  =  0; 

T't.  r  =  PDU_characcer_buf  f  er  ; 

while  {(bytes_left  •>  0)  &&  (bytes^written  >=  0))  /*  write  loop  */ 

byces_sent  =  write  ( socket_streaii:i,  ptr,  bytes_lefc); 

if  (bytes_sent  <  0)  by tes_writ ten  =  bytes_sent; 

else  if  (bytes_sent:  >  0) 

( 

byces_left  -=  bytes_sent; 

by tes^wri tten  +=  bytes_sent; 
ptr  +=  bytes_sent; 

) 

if  (TRACE) 

pr incf  ( '* di sbridge  send_PDU_to_other_host  loop  bytes  sent  =  %d\n", 
by  tes^sent )  ; 

; 

if  (by tes_writcen  <=  0) 

{ 

printf  {“disbridge  send_PDU_to_other_host  ()  send  failed,  " ) ; 
printf  (''%d  by  tes_wrir  ten\n“  ,  bytes_written)  ; 

else  if  (TRACE) 

prinrf  ("disbridge  send_PDU_to_other_host  total  bytes_wri tten  =  %d\n", 
by tes_wri tten) ; 

else  printf  ("disbridge  unsupported  PDUType  (%d) ,  not  passed  to  remoteXn", 
send_PDU_type ) ; 

’  end  serjd_PDU_t c_other_host  ;■  */ 
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by  t:es_recGived  +=  bytes_read; 
per  +=  bytes^read; 

I 

if  (TRACE) 

printf  (“disbridge  get_PDU_f rom_other_host  loop  bytes_read 
byte9_read)  ; 

/*  if  nothing  is  waiting  to  be  read,  break  out  of  read  loop 
if  (  (bytes__read  ==  0)  &&  (bytes^received  ==  0))  break; 

} 

if  f bytes_recei ved  <  0)  /*  failure  */ 

i 

if  (TRACE) 

printf  {"disbridge  get_PDU_f rom_other_host  read  failed,  " ) ; 
printf  ( "by tes_received  =  %d\n",  bytes_received) ; 

) 

return  (0);  /*  no  PDU  returned  »/ 

} 

else  if  (bytes_recei ved  ==  0)  /*  no  transfer  */ 

if  (TRACE) 

print f  (  "disbridge  get_PDU_f  roir[_other_host  read  received  0  bytesXn"); 
return  (0);  /*  no  PDU  returned  */ 

/*  else  (by tes_recei ved  >0)  /*  success  */ 

/ *  Cast  and  convert  buffer  to  return  remote^PDU  and  remote_PDY_type  */ 

rerT;ote_header  -  (PDUHeader  *)  reniote_PDU__buf f er ; 

*get_PDU  =  reinote_PDU_buf  f  er  ; 

*get_PDU_type  =  (PDUType)  reTnote_header->type; 

if  (TRACE) 

{ 

printf  ( "disbridge  get_PDU_f roin_other_host  read  bytes_received  =  %3d  \n’', 
bytes_received)  ; 

print f ( "disbridge  get_PDU_f roin_other_host  read  socket_length  =  %3d  \n", 
socket_length ) ; 

printf  ("disbridge  reitote_header  get_PDU__type :  %d\n'*,  *get_PDU_type)  ; 

printf  ("disbridge  get_PDU  reinote_PDU_buf  fer  =  [ " )  ; 
for  (i  =  0;  i  <  socket_length ;  1-*'+) 

printf  f"%x  ",  reitote_PDU_buf fer  [i]); 
printf  ( " ] \n" ) ; 

printf  ("disbridge  get_PDU  {)  remote_PDU_buf fer  =  [ " ) ; 
for  (i  =  0;  i  <■  socket__length;  i+-^) 

printf  ("%c",  reiTiote_PDU_buf f er  [i]); 
printf  ("]\n");  f flush  (stdout); 
ptr  =  *get_PDU; 

printf  ("disbridge  get_PDU  ()  get_PDU  =  ["); 

for  (i=0;  i  <  socket_length;  i*f+) 
printf  ("%c",  ptr  [i] ) ; 
printf  {"]\n");  f flush  (stdout); 

} 

return  (1);  /*  PDU  returned  */ 

}  /*  end  get_PDU_froin_other_host  ()  */ 

^/ •*•*■**■*■***★★★♦*★**★***★**♦****•*•********************************★****★♦**★★*★★**★/ 

void  print_PDU  (pdu,  type)  /*  from  John  Locke's  test^it.c  */ 

char  *pdu;  PDUTVpe  type;  /*  modified  to  stand  as  a  routine,  */ 

/*  no  other  changes  made  */ 

{ 

/*  example  PDUs  */ 

EntityStatePDU  *ESpdu; 


=  %d\n'‘, 

*/ 
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FirePDU 

*  Fpdu ; 

DetonationPDU 

*Dpdu ; 

ServiceRequestPDU 

*SRpdu; 

ResupplyPDU 

*Rpdu ; 

ResuppIyCancelPDU 

*RCpdu; 

Repai rCompletePDU 

*RC_pdu ; 

Repai rResponsePDU 

*RRpdU; 

CoIIisjonPDU 

Acti vat Request PDU 

*ARpdu; 

ActivatResponsePDU 

*AR__pdu  ; 

De  a  c  t  i  va  t  Requ  e  s  t  PDU 

* DR pdu ; 

Deact i va tResponsePDU 

*  ■ 

EiTii  t  terPDU 

*Epdu ; 

RadarPDU 

*Rad_pdu; 

Articula tParamsNode 

*APNptr; 

SupplyQtyNode 

*SQNptr; 

RadarSysteiTi 

*  rad_sys_ 

RadarSystemNode 

*RSN_ptr; 

RadarSys temsNode 

*RSNptr; 

Sr  oresNode 

*SNptr; 

Etii  t  terNode 

*ENptr; 

II luminedEnti tyNode 

*IENptr; 

/*  list  nodes  */ 


/*  Print  PDU  data  received  */ 

if  (TRACE)  printf ( “disbridge  print_PDU  {)  PDU  Type  =  %d\n",  type) ; 
switch  (type) 

( 

case  (OtherPDU_Type) : 

print  f  i  "Undef  ined  PDU  type  ( Other PDU_TVpe )  received .  Vn** )  ; 
break ; 

case  (FirePDU__Type)  : 

printfC'Got  %d: FirePDUXn",  FirePDU^TVpe }  ; 

Fpdu  =  (FirePDU  *)  pdu; 

printf ( "site:  %d\n" ,  Fpdu->f iring_entity_id . address . si te) ; 
printf  ( "range  :  %f  \n'' ,  Fpdu->range)  ; 
break ; 

case  ( EntityState?DU_T'ype;  : 
print  PLiU  ■:  char  *)  pdu  :  ; 
break ; 

case  (DetonatlonPDU^Type ) : 

printf ( "Got  %d:DetonationPDU\n" ,  DetonationPDU_Type) ; 

Dpdu  =  (DetonationPDU  *)  pdu; 
break ; 

case  (ServiceRequest PDU^TVP^) • 

printf ( "Got  %d: ServiceRequestPDUXn" ,  ServiceRequestPDU_Type) ; 
SRpdu  =  (ServiceRequestPDU  *)  pdu; 
printf ( "quantity  = 

SRpdu ->supply_qty_h€ad->supply_quantity . quantity) ; 

break ; 

case  (ResupplyOf ferPDU_Type) : 

printf ( "Got  %d:ResupplyOf ferPDUXn" ,  ResupplyOf f erPDU_Type) ; 

Rpdu  =  (ResupplyPDU  *)  pdu; 
printf ( "of fer  quantity  =  %f\n", 

Rpdu->supply_qty_head”>supply_quantity .quantity) ; 

break ; 

case  {ResupplyReceivedPDU_Type ) : 

printf ("Got  %d: ResupplyReceivedPDUXn" ,  ResupplyReceivedPDU  Type); 
Rpdu  =  (ResupplyPDU  *)  pdu; 
printf  { "received  quantity  =  %f\n‘', 

Rpdu->supply_qty_head->supply_quantity .quantity) ; 

break; 

case  (Resupply Cancel PDU_'IVpe)  - 

printf ("Got  %d:ResupplyCancelPDU\n" ,  ResupplyCancelPDU_Type) ; 
break; 

case  (RepairCompletePDU^Type) : 

printf ("Got  %d : RepairCompletePDUXn" ,  RepairCompletePDU^Type) ; 
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break ; 

case  ^RepairResponsePDU_Typei : 

printf{''Got  %d: RepairResponsePDUXn*' ,  RepairResponsePDU_Type )  ; 
break; 

case  {Coll isionPDU_Type) : 

printf{“Got  %d:CollisionPDU\n'' ,  CollisionPDU_TVpe) ; 
break ; 

/*  Experimental  PDU  Types  */ 
case  (Act! vatRequest PDU_Type) : 

printf("Got  %d:  ActivatRequestPDUXn "  /  ActivatReguestPDU_'TVpe)  ; 

ARpdu  =  (ActivatRequestPDU  ♦)  pdu; 
printf  ( ”radar_system  =  %d\n'', 

ARpdu ->radar_systems_head->radar_sy steins  .  radar_systein)  ; 
print  f  (" stores  qty  =  ARpdu->stores_head->stores  .  quantity ) ; 

print f ( " ID  =  %d\n " , 

ARpdu->articulat_params__head->articulat_parains .  ID)  ; 

break ; 

case  { Acti vatResponsePDU_Type) : 

print f ( “Got  %d : ActivatResponsePDUXn" ,  ActivatResponsePDU_Type ) ; 
break; 

case  (DeactivatRequestPDU_Type) : 

printf  (  “Got  %d:DeactivatRequestPDUXn‘' ,  DeactivatRequestPDU_Type)  ; 
break ; 

case  (DeactivatResponsePDU^Type) : 

printf ( “Got  %d:DeactivatResponsePDUXn“ ,  DeactivatResponsePDU_Type) ; 
break ; 

case  ( Emit  ter PDU_Type)  : 

printf  {  “Got  %d;  ErrdtterPDUXn" ,  Emi  tterPDU_Type}  ; 

Epdu  =  {Emit ter PDU  *)  pdu; 

print f  { "end ty_type  =  %dXn“,  Epdu->entity_type); 
printf ( “num_emitters  =  %dXn“,  Epdu->num_emitters) ; 
printf ( "emi tter_params .param_l  =  %dXn", 

Epdu->emitter_head->emitter .emi tter_params.param_l ) ; 

break ; 

case  {Rada rPDU_Type) : 

printf (“Got  %d: RadarPDUXn" ,  RadarPDU_Type) ; 

Rad_pdu  =  {RadarPDU  *)  pdu; 

printf  ( “location_to_entity  .X  =  %f\n'‘, 

Rad_pdu->radar_system_head->radar_system. location_to_enti ty .x) ; 
rad_sys_ptr  =  & {Rad_pdu~>radar_system_head->radar_system) ; 
printf ( “target_id. address .site  =  %XXn“; 

rad_sys_ptr->illumined_enti ty_head~>illumined_entity . 

target_id . address . site) ; 

break ; 
def aul t : 

printf (“ Invalid  PDU  type  received . Xn" ) ; 
break ; 


) 

}  /*  end  print_PDU  ()  */ 

^ic-^-itie-icic*ir-k*ie**icifkie*ic*****i(***-k*ic***-k****-k**ic*ic-icicic***ie***-k*****-k-k'k**-k-k*'t*******ic^ 
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Vm.  WORLD-WIDE  WEB  (WWW)  HYPERMEDIA  AND 
MULTICAST  BACKBONE  (MBone) 


A.  Introduction 

The  World-Wide  Web  (WWW)  project  has  been  defined  as  a  "wide-area 
hypermedia  information  retrieval  initiative  aiming  to  give  universal  access  to  a  large 
universe  of  documents"  (Hughes  94).  Fundamentally  the  WWW  combines  a  name 
space  consisting  of  any  information  store  available  on  the  Internet  with  a  broad  set  of 
retrieval  clients  and  servers,  all  of  which  can  be  connected  by  easily-defined  hypertext 
markup  language  (.html)  multimedia  links.  This  globaUy-accessible  combination  of 
media,  client  programs,  servers  and  hyperlinks  can  be  conveniently  utilized  by  humans 
or  autonomous  entities.  The  Web  has  fundamentally  shifted  the  nature  of  information 
storage,  access  and  retrieval  (Hughes  94)  (Berners-Lee  94a,  94b). 

Universal  Resource  Locators  (URLs)  are  a  key  WWW  innovation.  A  block  of 
information  might  contain  text,  document,  image,  sound  clip,  video  clip,  executable 
program,  archived  dataset  or  arbitrary  stream.  If  that  block  of  information  exists  on 
the  Internet,  it  can  be  uniquely  identified  by  host  machine  IP  address,  publicly  visible 
local  directory,  local  file  name,  and  type  of  client  needed  for  retrieval  (such  as 
anonymous  ftp,  hypertext  browser  or  gopher).  Ordinarily  the  local  file  name  also 
includes  an  extension  which  identifies  the  media  type  (such  as  .ps  for  PostScript  file 
or  .rgb  for  an  image).  Thus  the  URL  completely  specifies  everything  needed  to 
retrieve  any  type  of  electronic  information  resource.  Example  URLs  appear  in  the  list 
of  references,  e.g.  (Hughes  94). 

The  Multicast  Backbone  (MBone)  permits  several-to-many  simultaneous 
high-bandwidth  communications  streams  across  the  Internet  Detailed  information  on 
MBone  applications  and  use  appears  in  (Macedonia,  Brutzman  94).  A  customized 
session  directory  (sd)  configuration  file  .sd.tcl  is  provided.  This  configuration  file 
adjusts  the  sd  interface  at  run  time  to  enable  automatic  DIS  connection  of  the 
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underwater  virtual  world  viewer  and  automatic  spawning  of  a  Mosaic  browser 
connection  to  the  NPS  AUV  WWW  Home  Page.  Thus,  once  a  system  has  been 
configured  using  free  software  for  Mosaic,  MBone  and  this  project,  connection  to  an 
already-running  underwater  virtual  world  requires  only  a  single  button  click  from  any 
compatible  workstation  on  the  Internet.  Global  connectivity  and  ease  of  use  are  thus 
demonstrated  in  order  to  further  encourage  remote  collaborative  research  and 
large-scale  widely-distributed  virtual  worlds. 


B.  NFS  AUV  World-Wide  Web  Home  Page 

Hot  links  to  other  home  pages,  servers  or  media  are  portrayed  by  underlined 
text  in  the  home  page.  Line  breaks  and  page  layout  are  handled  automatically  by  the 
browser  according  to  window  and  font  size. 


NFS  Autonomous  Underwater  Vehicle  (AUV)  World-Wide  Web  Home  Page 

ftp  Jltaurus .  cs.nps.navy.mil/pnblauv/auv.html 

NPS  Autonomous  Underwater  Vehicle  (AUV) 


NFS  AUV  publication  abstracts  and  anonymous  ftp  server 
NPS  AUV  graphics  rendering  and  a  wireframe  rendering 


NPS  AUV  Underwater  Virtual  World 


fin2er  auv&dude. cs.nvs.navv.mil  for  a  sample  mission  report. 


A  free  tar  archive  of  the  virtual  world  software  is  here;  auv-uvw.tar.Z.  including 
source  code  and  executable  binaries  that  run  on  Silicon  Graphics  (SGI)  workstations. 

A  user  installation  guide  is  available.  Please  let  me  know  if  you  need  help  installing 
the  virtual  world.  Research  collaboration  is  welcome. 

Here  is  another  sample  NPS  AUV  mission  report  run  in  the  virtual  world. 
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Additional  supporting  papers  and  programs  used  with  the  virtual  world  software  are 
distributed  separately: 

•  "From  virtual  world  to  reality:  desi2nine  an  autonomous  underwater 
robot"  paper  and  slides. 

•  Vehicle  telemetry  postscript  plots  (20  pages)  for  the  SIGGRAPH 
mission. 

•  Mission  script  syntax  for  NFS  AUV  execution  level  control: 
mission.scriptHELP  file. 

•  WWW  line  mode  World-Wide  Web  browser  distribution  page  for  the 
WWW  anonymous  ftp  directory,  www  is  used  to  make  World-Wide 
Web  queries  from  inside  the  virtual  world.  This  program  is  needed  if 
you  want  live  text  to  speech  capability.  Put  www  in  the  /dynamics 
directory  or  have  your  system  administrator  install  it. 

•  gnuplot  plotting  program  distribution  directory  (and  FAQ  page), 
gnuplot  is  used  to  plot  robot  telemetry  results.  This  program  is 
needed  if  you  want  a  plotting  capability.  Put  it  in  the  /execution 
directory  or  have  your  system  administrator  install  it. 

•  MB  one  Multicast  Backbone  connection  information.  An  MB  one 
connection  is  needed  if  you  want  to  participate  in  worldwide 
audio/video/DIS  multicasts  with  the  underwater  virtual  world.  If  you 
are  a  local  user  on  the  gravy  cs.nps.navy.mil  subnet,  you  don’t  have 
to  download  the  full  distribution  but  instead  can  copy  the  MBone 
session  directory  (sd)  configuration  file  .sd.tcl  and  .111311030  mosaic 
initialization  file  to  your  root  directory,  and  then  paste  the  following 
.cshrc  aliases  into  your  root  directory  .cshrc  file.  After  you  source 
•cshrc  you  are  ready  to  run  sd  and  mosaic. 
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The  NPS  AUV  Underwater  Virtual  World  also  appeared  at  The  Edge  exhibition  at 
SIGGRAPH  94,  July  26-29  in  Orlando  Florida  USA.  There  was  a  simultaneous 
MBone  multicast  on  the  worldwide  Internet  all  that  week.  Information  resources  from 
that  exhibit  include: 

•  project  abstract. 

•  collaborator  list. 

•  people  pages. 

•  1000-word  description  and  proposal  for  exhibition  in 

•  The  Edge  exhibition  at  SIGGRAPH  94.  and 

•  information  regarding  the  audio/video  multicast  on  the  Internet 
MBone. 


The  Universal  Resource  Locator  (URL)  for  this  home  page  is 
ftp://taurus.cs.nps.navy.mil/pub/auv/auv.html 


C.  Hypertext  Markup  Language  (HTML)  Syntax  Summary 

The  following  HTML  syntax  summary  will  help  decipher  most  of  the  home 
page  document  which  follows  in  the  next  section.  The  key  feature  of  HTML  is  that  it 
describes  document  structure:  images,  the  relative  emphasis  of  text  entries  and  links 
to  other  documents  or  files  via  URLs  (NCSA  94b). 

The  rendered  format  of  an  HTML  document  can  vary  depending  on  the 
browser  used  to  access  it.  This  permits  both  graphics  terminals  and  character-based 
terminals  to  each  access  and  display  a  document,  preserving  contextual  links 
throughout.  URL  media  format  is  indicated  by  the  filename  extension.  The  ability  to 
display  movies,  play  sounds,  and  utilize  other  media  depends  on  the  capabilities  of  the 
connecting  machine,  the  presence  of  ’viewer’  programs  which  can  receive  and  output 
the  media  after  a  handoff  from  the  browser,  and  proper  links  between  MIME  types 
and  browsers  as  configured  in  the  user’s  .mailcap  file. 
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The  following  is  a  short  synopsis  of  key  HTML  markups.  Markup  tags  are 
generally  case  insensitive.  Blanks  and  skipped  lines  between  markup  tags  have 
no  effect. 

See  URL  http:/lwww.ncsa.uiuc.edulGenerallInternetlWWWIHTMLPrimer.html 
for  detailed  information  on  HTML  syntax. 

<B> 

bold  text  </B> 

<1> 

italics  text  </I> 

<P> 

inserts  a  paragraph  break. 

<HR> 

places  a  horizontal  rule  before  the  next  item: 

<A  IMG= 

'universal  resource  locator"> 
places  an  inline  image  on  the  page. 

<A  HREF 

="universal  resource  locator">  button  text> 

places  button  text  on  the  screen  which  retrieves  "universal  resource 
locator"  when  selected.  Inline  images  can  be  used  in  place  of  text. 

<ul> 

starts  an  unnumbered  list 

<li> 

starts  a  list  item 

<li> 

starts  another  list  item 

</ul> 

ends  an  unnumbered  list 

Figure  5.  Hypertext  Markup  Language  (HTML)  Syntax  Summary. 
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D.  auv.html  NPS  AUV  Home  Page  (Hypertext  Markup  Language) 


<TITLE>  NPS  Autonomous  Underwater  Vehicle  (AUV)  World-Wide  Web  Home  Page</TITLE> 
</HEADER> 

<H1>  <A  NAME=AU\^>  NPS  Autonomous  Underwater  Vehicle  (AUV)  </Hl> 

<A  HREF=  "ftp://taurus.cs.  nps  .  navy.mil/pub/auv/auv .  iv .  gif  "xIMG 
SRC=*’ftp: //taurus  .cs.nps. navy  . mil/pub/auv/auv.iv. gif "  ALT="NPS  AUV  logo"></A> 

<A  HREF=  "  f  tp :  /  /taurus  .  cs  .nps  .  navy  .mil /pub/ auv/nps_auv.  au  "xIMG 
-  SRC=  *' ftp:  / /taurus  ,cs.  nps  .  navy  .mil/pub/mosaic/sound.xbm"  ALT="sound  icon‘'x/A>. 

..  <A 

HREF= "http : / /www_tios . cs . utwente . nl : 8001 /say/?Naval+Postgraduate+School , Autonomous+Und 
trwater^Veiiicle"><IMG  SRC="ftp:  /  /taurus  .cs .  nps .  navy  .mil /pub/mosaic /sound. xbm" 

ALT="  sound  icon''></A> 

(about  <A  HREF  =  '‘http://www_tios.cs.utwente.nl/say/?">  Say...</A>) 

<H2>  NPS  AU\-  <A  HREF  =  "  ftp  ;/ /taurus  .  cs  .nps  .navy  .mil /pub/ auv/AUVPAPERS"  > 

publication  abstracts</A>  and 

<A  HREF  =  " ftp : //taurus . cs .nps .navy .mil /pub/auv">  anonymous  ftp 
server<i'>  (  ftp  :/ /taurus  .  cs.nps  .navy.mil/pub/auv)  </ix/A>  </H2> 

<E>  NPS  AUV  </B>  <A  HREF  =  " ftp :// taurus . cs . nps . navy  .mil /pub/auv/auv . iv. rgb. Z " > 
araohics  rendering< /A>  and  a 

<A  HREF  = 

"  ftp;  //taurus  .cs,  nps  .navy  .mil  /pub/auv/auv .  iv.  wireframe .  rgb.  Z*'>  wireframe  rendering</A> 

>-P> 

':hr> 

<H1>  <A  NAME= “AUV-UVW">  NPS  AUV  Underwater  Virtual  World</Hl> 

<A  HREF  =  "f  tp: //taurus.cs.nps.navy.mil/pub/auv/auv-uvw.gif  “xIMG  SRC  = 
"ftp: //taurus  .cs.nps. navy  . mil/pub/auv/auv-uvw. gif "  ALT='‘AUV  Underwater  Virtual  World 
1  IT;  a  g  e  "  > ' '  /  A  >  <  P  > 

Finger  the  NPS  AU\^  account  <A  HREF  = 

"http  :  /  /WWW .  cs  .  indi  ana  .  edu /finger /gateway  ?auv(adude .  cs  .  nps  .navy.mil"  xi>  (finger 
auv@dude .  cs  .  nps  .  navy  .mil )  </ix/A>  for  a  sample  mission  report. <P> 

<B>  <"A  HREF  =  "ftp: //taurus. cs.nps, navy .mil/pub/auv/auv-uvw. tar. Z">A  free  tar 

archive  of  the  virtual  world  software  is  here:  auv-uvw. tar . Z</A>  </B> 

including  source  code  and  executable  binaries  that  run  on  Silicon  Graphics 
CSGl!  worksta ti ons . <P> 

A  <A  HREF  = 

“ f tp : //taurus .cs.nps. navy .mil /pub /auv/ auv-uvw. virtual -world. INSTALL ">user  installation 
guide</A> 

is  available.  Please  let  me  know  if  you  need  help  installing  the  virtual 
world.  Research  collaboration  is  welcome.  <P> 

Here  is  another  sample  <A  HREF  = 

" f tp : //taurus .cs . nps . navy .mil /pub /auv /auv-uvw. virtual -world. README ">NPS  AUV  mission 
report</A>  run  in  the  virtual  world.  <P> 

Additional  supporting  papers  and  programs  used  with  the  virtual  world 
softare  are  distributed  separately: 

<ul> 

<li>  <A  HREF  =  "ftp: //taurus. cs.nps. navy. mil/pub/auv/aaai92ws.ps.Z"xi>"From 
virtual  world  to  reality:  designing  an  autonomous  underwater  robot "</i>  paper</A>  and 
<A  HREF  = 

•ftp: //taurus .cs .nps. navy .mil/pub/auv/aaai92ws . slides .ps . Z">slides</A>. 
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<ii,-  <A  HREF  = 

"  f  cp :  //taurus  .cs  .  nps  .  navy  .mil  /pub/auv/AUV^telemetry  .ps  .  Z ‘‘><B>Vehicle  teleinetry</B> 
pc?C5cripc  plots  (20  pages)  for  the  SIGGRAPK  mission</A>, 

■  1:  ■  -A  HREF  =  “ f tp : //taurus . cs .nps .navy .mil/pub/auv/mission. script .HELP"> 
Mission  script  syntax  for  NPS  AUV  execution  level  control</A>:  mission. script .HELP 


<:li>  <A  HREF  = 

*'  http :/  /  inf  o .  cern  .  ch/hypertext/WWW/LineMode /Defaults/Distribution .  html  ''><B>www</B> 
line  mode  World-Wide  Web  browser</A>  distribution  page  for  the 

<A  HREF  =  " ftp : //inf o. cern . ch/pub/www/src/ “><B>www</B>  anonymous  ftp 
directory</A> . 

<b>www</b>  is  used  to  make  World-Wide  Web  queries  from  inside  the  virtual 
Ti.is  program  is  needed  if  you  want  live  text  to  speech  capability. 

Put  --b>www<:/b>  in  the  /dynamics  directory  or  have  your  system  administrator 

^]':Stai.j.  it. 

<li>  <A  HREF  =  “http : //WWW. cs . dartmouth . edu/gnuplot/ "> 

*  B  •gnupioc-  /B>  plotting  program</A>  distribution  directory  {and 

-A  HREF  =  ''ftp://ftp.dartmouth.edu/pub/gnuplot/faq/gpt_faq.html">  FAQ 

page*'/A'-.  . 

-:b->qnuplct</ b‘>  is  used  to  plot  robot  telemetry  results.  This  program  is 
nr^-dtri  -f  you  want  a  plotting  capability. 

Put  it  in  the  /execution  directory  or  have  your  system  administrator 

install  it. 


<li.'  <A  HREF  =  f  tp  : //taurus  .  cs  .nps  . navy  .mil/pub/mosaic/mbone  .html " ><B>MBone</E> 
Multicast  Backbone</A'>  connection  information. 

.An  MBone  connection  is  needed  if  you  want  to  participate  in  worldwide 
audio/videc/ DIE  multicasts  with  the  underwater  virtual  world.  <P> 

If  you  are  a  local  user  on  the  grav^^a .  cs  .nps .  navy  .mil  subnet,  you  don't 
have  to  download  the  full  distribution  but  instead  can  copy  the  MBone  session  director 
i<b>sd--/b>;  configuration  file 

vA  HREF  =  "ftp : //taurus . cs .nps .nav>^  .mil/pub/auv/ .sd. tcl "> 

-•B>  .  sd .  tcl*u  B  -</A>  and 

-.A  tiriEF  =  "  ftp  : //taurus  .  cs  .nps  .na^vy  ,mil/pub/auv/ .mailcap"><B>,mailcap</B> 
Hiosaic  initialization  file</A>  to  your  root  directory,  and  then  paste  the  following 
<A  HREF  = 

"ftp: / /taurus . cs .nps .navy .mil /pub/auv/ . cshrc-excerpt " ><B> . cshrc</B>  aliases</A>  into 
your  root  directory  <B> . cshrc</B>  file.  After  you  <B>source  .cshrc</b>  you  are  ready 
to  run  <b>sd</b>  and  <b>miosaic</b> . <P> 

<  /  u  1  > 


<A  HREF  =  "ftp: //taurus. cs .nps. navy, mil/pub/auv/pool-search. gif ">  <IMG  SRC 
=  "ftp://taurus.cs.nps.navy.miil/pub/auv/pool-search.gif"  ALT="AUV  pool  search 
imiage  v  A><P> 

The  <B>NPS  AUV  Underwater  Virtual  World</B>  also  appeared  at 
<A  HREF  =  "http://www.vsl.ist.ucf.edu/projects/edge/edge.html"> 

<B>The  Edge</B></A>  exhibition  at 

•.A  HREF  =  "gopher://siggraph.org/00/conferences/siggraph94/gip/gip.txt"> 
uE-'  SIGGRAPH  94</B></A>,  July  26-29  in  Orlando  Florida  USA. 

There  was  a  simultaneous 
■^A  HREF  = 

" f tp : / /taurus . cs . nps . navy .mil /pub/auv/SIGGRAPH94/SIGGRAPH94-mbone .html ">  MBone 
multicast  on  the  worldwide  Internet</A>  all  that  week. 

Information  resources  from  that  exhibit  include: 

<ul> 

<Ii>  <A  HREF  =  "ftp://taurus.cs.nps.navy.mil/pub/auv/SIGGRAPH94/abstract.txt"> 

project  abstract</A>, 

<ii>  <A  HREF  =  "ftp://taurus.cs.nps.navy.mil/pub/auv/SIGGRAPH94/contacts.txt"> 

collaborator  list</A>, 

<li>  <A  HREF  =  "ftp://taurus.cs.nps.navy.mil/pub/pratts/home.html"> 

people  pages</A>, 

<li>  <A  HREF  = 
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"ftp : //caurus .cs . nps . navy .mil /pub/auv/SIGGRAPH94 /edge_description . txt “>  1000-word 
de^ci'ipt i  on</A>  and 
<A  HREF  = 

"  ftp :  /  /caurus  .  cs  .  nps  .  navy  .mil  /pub/auv/SIGGRAPH94/edgG_proposal  .ps  .  Z  '•>  proposal</A> 
for  exhibition  in 

<ii-.'  <A  HREF  =  '‘http://www.vsl.ist.ucf.edu/projects/edge/edge.html"> 

<B>The  Edge</B></A>  exhibition  at 

<A  HREF  =  "gopher: //siggraph.org/00/conferences/siggraph94/gip/gip.txt"> 
<B>  SIGGRAPH  94</B></A>,  and 
<Ii>  information  regarding  the  <A  HREF  = 

" f tp : / / taurus . cs . nps . navy .mil /pub/auv/SIGGRAPH94/SIGGRAPH94-mbone .html *>audio /video 
niult.^casc  on  the  Internet  MBone</A>. 


<H1-'  IEEE  Oceanic  Engineering  Society  AUV  94  Conf erence</Hl> 

-:A  HREF  =  "ftp://taurus.cs.nps.navy.mil/pub/auv/auv94.logo.gif">  <IMG  SRC  = 
''ftp://taurus.cs.nps.navy.mil/pub/auv/auv94.logo.gif"  ALT="AUV  94  logo“></A>  <P> 

AW  94  took  place  July  19-20  1994  in  Cambridge  Massachusetts  USA. 

<ui> 

<ii>  <A  HREF  =  " ftp : //taurus . cs .nps .navy .mil /pub/auv/auv94 .program. txt "> 
Conference  Program  in  text</A>  and 

<A  HREF  =  "ftp://taurus.cs.nps.navy.mi1/pub/auv/auv94.ps.Z"> 
pcstscript< /A>  formats, 

<Ii>  <A  HREF  =  "ftp://taurus.cs.nps.navy.mil/pub/auv/auv94video.info"> 

Video  Proceedings  broadcast  inf orma ti on</A> ,  and  the 

<Ii>  <A  HREF  =  "ftp://taurus.cs.nps.navy.mi1/pub/auv/auv94video.ps.Z"> 

Video  Proceedings  abstract  booklet</A>. 

We  were  able  to  broadcast  the  video  proceedings  and  selected  sessions  over 

tne  MBcne . 

Conference  e-mail  queries  may  be  sent  to  <i>auv94@ieee . org</i>  <P> 


<hr> 

<H2>  Other  good  stuff!  </H2> 

•"A  HREF  =  "ftp://ftp.nps.navy.mil/pub/usw/av_mcm.html">  <B>Symposium  on 
Autonomous  Systems  in  Mine  Countermeasures</B></A>  at  NPS  April  4-6  1995.  <P> 

Unmanned  Untethered  Submersible  Technology  (UUST)  93  <A  HREF  = 

" ftp: //taurus . cs.nps . navy .mil /pub/auv/uust93_video_order . form">  video  proceedings 
aos tracts- /A>  with  order  form  <P> 

<A  HREF  =  "http://www.cs.wisc.edu/-pruyne/os9faq.html">  <B>0S-9 
FAC-  /E  ■•■/A>:  the  OS-9  real-time  operating  system  is  what  we  use  in  the  vehicle. 

Also  pertaining  to  OS-9  is 

<A  HREF  =  "http://iglou.com/-stevenw/windsorhome.html">  Windsor  Systems 
Home  Paqe</A>.  <P> 

<A  HREF  =  "ftp://taurus.cs.nps.navy.mi1/pub/auv/healey.rgb.Z">  Tony 
Healey<'/A>  (healey@lex.me.nps.navy.mil)  , 

<A  HREF  =  "ftp://taurus.cs.nps.navy.mi1/pub/auv/mcghee.rgb.Z">  Bob 
McGhee</A>  (mcghee@cs.nps.navy.mil),  and 

<A  HREF  =  "ftp://taurus.cs.nps.navy.mi1/pub/auv/brutzman.rgb.Z">  Don 
Brutzman</A>  (brutzman@nps . navy .mil ) 

<A  HREF  =  "http://www.cs.indiana.edU/finger/gateway7brutzman@nps.navy.mil"> 
contact  info</A>  <P> 

return  to  <A  HREF=" ftp: //taurus .cs .nps. navy .mil/pub/mosaic/nps_mosaic .html#TOC“> 
NPS  Home  Page  cable  of  contents</A> 


</ul> 
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_ <p> 

The  Universal  Resource  Locator  (URL)  for  this  home  page  is 
ftp://taurus.cs.nps.navy.mil/pub/auv/auv.html  </B>  <P> 

NPB  AU\'  Wcrld-Wide-Web  Home  Page:  <A  HREF  = 

"itp:  taurus  .  cs .  nps  .  na\y' .mil /pub/auv/brutzman .  rgb.  Z’'>.  Don  Brut2man</A> 

(brut 2man(5nps  .nav;^'  .mil ) 

<A  HREF  = 

"lutp;//www.cs. Indiana. edu/finger/gateway?brutziiian@nps. navy .iTtil">  contact  info</A>  (18 

October  94) 

••/ADDRES£> 
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E. 


.sdJcl  Multicast  Backbone  (MBone)  Configuration  File 


# 

# 

# 

# 

# 

# 

# 

¥ 

# 

# 

# 

# 

It 

# 

# 

I; 

# 

H 

n 

# 

# 

# 

H 

# 

It 

it 

# 

It 

# 

It 


SHeader:  sd_start . tcl , v  1.4  (Van  Jacobson  LBL) 

nOTE:  This  file  was  modified  from  the  default  .sd.tcl 

to  support  automatic  execution  of  multicast  DIS 
virtual  world  programs  for  the  NFS  AUV  and  NPSNET, 
and  to  automatically  spawn  mosaic  when  a  UKL  is 
present  in  the  session  advertisement. 

Please  report  additions,  comments  and  corrections  to 
Don  Erutzman,  brutzman@nps.navy.mil 

last  modified:  19  OCT  94 

URL  of  this  example  file  is: 

ftp://taurus.cs .nps . navy^  .mil/pub/auv/ .sd.tcl. auv-uvw 

tcl  'hocks'  invoked  when  sd  takes  some  action  on  a  session. 

sd  v;ill  invoke: 

start_sessi on  when  the  user  asks  to  'open'  (start)  a  session 
create_session  just  after  the  user  creates  a  new  session 
heard_session  when  announcement  for  a  session  is  first  heard 
delete_session  when  the  user  or  a  timeout  deletes  a  session 

when  any  of  the  above  are  invoked,  the  global  array  sd_sess 
contains  all  the  information  about  the  session  to  be  started: 

sG _ sess  inaiTie) 

sd_se3s ;description ) 

2 G__ sess  .  aGGress  < 
sd_sess ( t tl : 
sd_:5e£s  ; creator  • 

SG_sess ;creator_id; 
sd_sess (source^id) 

2d_sess (arrival_time ; 
senses s ( start_t ime  j 
sd  sess end_t  ime 

sd_sess : attributes)  (list  of  session  attributes) 

sd_sess  (iTfedia  j  (list  of  media  names) 

For  each  media  namie  there  is  an  array  containing  the  information 
ahc. ’^;r  that  med-a: 
sd_$miedia  (port ) 
sd_$niedia  (conf_id) 

ci  tmedia (attributes)  (list  of  media  attributes) 

Media  and  session  attributes  are  strings  of  the  form  “nairie"  or 
"name : value" . 

Some  global  state  information  is  available  in  array  sd_priv: 
sd_priv (audio)  (=  0  if  audio  disabled  with  ~a) 

sd _priv (video)  (=  0  if  video  disabled  with  -v) 

sd_priv (whiteboard)  (=  0  if  wb  disabled  with  -w) 


proc  start_session  {}  { 

global  sd_sess  sd_j)riv  mosaic 

#  thanks  to  Bill  Fenner  of  Navy  Research  Lab  for  posting  this  addition. 

#  start  up  Mosaic  if  there  is  a  URL  in  the  description. 

if  {[regexp  {  [a-zA-Z]  + : // [ '^  ]  +  }  $sd_sess  (description)  url]}  { 
exec  $mosaic  -home  $url  & 

} 


#  invoke  the  appropriate  start  proc  for  each  of  the  media 
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#  if  such  a  proc  exists  and  that  media  is  enabled, 
foreach  m  $sd_sess (media )  { 

if  {  [llength  [info  proc  start_$md  ]  $sd_priv($m)  )  { 
start_$m. 


proc  starc_audio  {}  { 

global  sd_sess  sd_audio 
set  audicfmt  " " 
set  packetfmt  "-n" 
foreach  a  $sd_audio (attributes }  { 
case  $a  { 

fmt : *  {  set  audiofmt  [string  range  $a  4  end]  } 
vt  {  set  packetfmt  “-v“  } 


set  confaddr  [format  *’%s/%s/%s/%s/%s''  $sd_sess  (address)  \ 

$sd_audio (port)  $sd_audio (conf^id)  Saudiofmt  $sd_sess { ttl ) ] 

global  vac 

exec  $vat  -C  $sd_sess (name;  Spacketfmt  Sconfaddr  & 

} 

proc  starC_video  {}  { 

g local  sd_sess  sd_videc 
set  videofmt  "nv" 
foreach  a  $sd_video (attributes)  { 
case  $  a  { 

fmt:*  {  set  videofmt  [string  range  $a  4  end]  } 

] 

case  S video fmt  { 
nv  1 

global  nv 

exec  $nv  -ttl  $sG_sess ( ttl )  $sd_sess ( address)  \ 
$sd_video (port )  & 

) 

ivs  { 

global  ivs 

exec  Sivs  -a  -r  -T  $sd_sess ( t tl )  \ 
dest  $sd_sess (address )  & 

} 

jpg  ( 

global  imm 

exec  $imm  -p  $sd_video (port )  -I  $sd_sess (address)  \ 
-ttl  $sd_sess ( ttl )  -n  $sd_sess (name)  & 


proc 


& 


start_whiteboard  {}  { 

global  sd_sess  sd_whi teboard  wb 
set  orient  "-1" 

foreach  a  $sd_whi teboard (attributes)  { 
case  $a  { 

orient iportrait  {  set  orient  -p  } 
orient : landscape  {  set  orient  -1  } 
orient : seascape  {  set  orient  +1  } 
orient :dis~npsnet  { 
global  nps 

exec  $nps  -F  "conf ig , trg“  -p  $sd_video (port )  \ 
-I  $sd_sess (address)  -t  $sd_sess ( ttl ) 

#  do  not  also  execute  whiteboard 
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return 


$sd_whi teboard (port ) 


} 

orient : di s-auv-uvw  { 

global  dis_auv_uw 
global  dis_auv__uvw_dir 
cd  $dis_auv_uvw_dir 

exec  $dis__auv_uw  -port 

-address  $sd_sess  (address)  & 
cd 

#  do  not  also  execute  whiteboard 
return 


} 

global  xpsview 

#  running  xpsview  is  a  bug  fix  to  prevent  irix  5.2  crash 
puts  stdout  “  ’’ 

puts  stdout  "Spawning  duminy  xpsview  to  avoid  whiteboard  bug  crash  under 
exec  $xpsview  & 

exec  $wb  -t  $sd_sess ( tt 1 )  -C  wb : $sd_sess (name)  $orient  \ 

$sd_sess (address) /$sd_whi teboard (port )  & 


proc  create_session  {}  { 
) 


proc  neard_session  {}  { 

} 

proc  deleters ess ion  { )  { 

} 

^  sen  up  media  option  menus  for  new  session  windows. 

sec  sd_menu (whiteboard)  "orient:  portrait  landscape  seascapeX 

dis-npsnec  dis-auv-uvw" 

set  sd_menu  (audio)  "  f mt :  pern  pcm2  pcm4  idvi  dvi2  dvi4  gsm  lpc4  nvt" 

set  sd^menu f vi deo )  "fmt:  nv  ivs  imm" 

#  set  up  the  command  names 

#  Edit  these  to  match  your  system  locations  &  filenames,  if  necessary. 
n  The  version  of  mosaic  that  you  point  to  must  not  have  a  precompiled 

#  iocal  home  page  hard-wired  in  the  binary,  or  it  won't  redirect. 

" /n/elsie/work3/macedoni/ video/ vat" 

" /n/elsie/work3/macedoni/video/nv" 

" /n/elsie/work3 /macedoni/video/ivs" 

" /n/elsie/work3/macedoni/video/wb_src/wb" 

" /n/elsie/work3/macedoni/video/imm" 

set  mosaic  " /n/dude/work/brutzman/xmosaic/Mosaic-sgi • 

set  xpsview  xpsview 

set  npsnet  •*  /n/bossie/workS/npsnetlV/npsnetlV 

set  dis_auv_uvw_dir  " /n/dude/work/brutzman/auv-uvw" 

set  dis_auv_uvw  viewer 


set  vat 
set  nv 
set  ivs 
set  wb 
set  imm 
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F.  .mailcap  Configuration  File  for  Mosaic  Multimedia  Viewers 

####################################################### 

a::-:b.3:2  mailcap  file  for  mosaic  on  NPS  gravy  net  Don  Brutzman  18  OCT  94  # 

^  global  Unix  system  location:  /usr/local/lib/mosaic/mailcap 

#  users  can  put  their  own  preferences  in  -yourname/ .mailcap 

^  edit  directory  paths  as  necessary  for  your  machine 

# 

#  reference :  http : / /www.ncsa .uiuc . edu / SDG / So f tware /Mosaic /Docs /mail cap .html 

# 

#  URL  of  this  example  file  is: 

^  tip: / /taurus . cs .nps .navy .mil/pub/auv/ .mailcap .auv-uvw 

# 

appi i ca ti on /postscript ;  xpsview  %s 
4?  ########## 

#  If  you  wish  to  use  xplaygizmo  (  ftp://ftp.ncsa.uiuc.edu/Mosaic/misc/  ) 

M  video /mpeg;  xplaygizmo  -p  mpeg_play  %s 

^  t: -ornate  mpeg_player  which  includes  looping  capability: 

V  i  dec  /mpeg ;  /  n  /dude  /work  /bru  t  zman  /xmiosa  i  c  /mpeg_pl  ay  er  %  s 

^  :  don't  tnink  xpiaygizmic  is  needed  for  small  audio,  but  it  is  workable. 

41  It  is  helpful  on  longer  files  such  as  Internet  Talk  Radio. 

a-^dio/*;  /n/dude/work/brutzman/xmosaic/xplaygizmo  -p  -q  sfplay  %s 


# 

4t  STILL  NEEDED:  dvi  viewer  (such  as  the  one  which  works  for  mosaic  on  grus) 
appiication/x-dvi ;  xdvi  %s 


??  Note:  besides  Mosaic,  mailcap  files  are  used  by  MIME-capable 

#  multimedia  mail  handlers 

#  Final  note:  be  sure  to  select  /Options  _Reload_Conf ig  Files  from  the  menu 

44  for  any  changes  to  be  effective  in  your  current  session. 
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G. 


cshrc  Excerpts:  Login  Configurations  for  MBone  and  Mosaic 


##*#### #*################ ###################################*#### 


#  mbone  aliases/pachs  [from  -brutzman/ . cshrc] 

#  sd  to  start  the  session  directory  tool,  everything  else 

#  is  invoked  from  the  sd  menu 


set  path  =  ($p3th  -macedoni /video  -macedoni /bin) 

set  path  =  ($path  --macedoni /video/wb_src  ~macedoni/video/mm_„src ) 


a  ^  t  a s  so 
alias  vat 
alias  nv 
alias  wb 
alias  XV 
alias  imm 
alias  imjT,2xv 
ai^as  ivs 


' -macedoni /video /sd 
'  '-macedoni  /video /vat ' 
'-macedoni /video/nv' 

' -macedoni /video /wb_src/wb' 

' -macedoni /video/xv^ 

' -macedoni /video/imm' 
'--macedoni  /video/mm2xv ' 

' '-macedoni  /video/ivs ' 
normally  imm  goes  full  screen, 


#  session  directory 

#  visual  audio  tool 

#  net  video 

#  white  board 

#  xview 

#  image  monitor  tool 

#  image  monitor  =>  xview 

#  INRIA  videoconferencing  system 
imm  can  use  iinm2xv  for  a  window 


#  also  copy  the  file  -^brutzman/ .  sd.  tcl  to  your  home  account 

#  sc  that  the  tools  are  initially  set  up  properly! 

#  subscribe  to  local  NFS  list  npsmbone@nps.navy.mil  if  you  are  interested 


#  mosaic/wcrld-wide-web  browser  aliases/paths  -  for  gravynet  users 


set  path  =  (Spath  --brut zman/xmosaic ) 
alias  mosaic  '-brut zman/xmosaic/Mosaic-sgi 

ftp : / / taurus . cs -nps .navy , mil /pub/mosaic /nps_mosaic .html#TOC 
alias  wwvj  ' -brut zman/xmosaic /www 

f tp; / /taurus . cs .nps .navy .mil /pub/mosaic /nps_jnosaic .html  ' 


alias  lynx  ' -brut zman/xmosaic/lynx 

f tp : / /taurus . cs .nps . navy.mil/pub/mosaic/nps_mosaic.html  ' 
also  copy  zh^  .mailcap  file  to  initialize  mosaic 
cp  —brutzman/  .mailcap  .mailcap 

################################################################# 

#  URL  of  this  example  file  is: 

#  ftp://taurus.cs. nps . navy . mi 1 /pub/auv/ . cshrc-excerpt . auv-uvw 
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